Allow pose/joint targets for motion functions as in URScript

[urfeex]

Feature summary

In URScript movej and movel can take both, Cartesian and joint-based targets. For the client library we currently have the communication structure such that with a movej motion the 6 values are interpreted as joint values, with a movel they are interpreted as a pose.

For this to work, we would have to extend the communication protocol to specify whether the target values correspond to a joint target or a pose target. With this information, we can act on the external_control script accordingly.

This is planned for later in 2025 but if someone wants to take action on this, that's welcome.

Related issues

• writeJointCommand'problem #313

Tasks

To complete this issue involves

• Implement the feature
• Make documentation
• Make Unit test
• Make example
• Test on real hardware

[hestrro]

There are several changes：

external_control.urscript:
MOTION_TYPE_MOVEJ = 0
MOTION_TYPE_MOVEL = 1
MOTION_TYPE_MOVEP = 2
MOTION_TYPE_MOVEC = 3
MOTION_TYPE_MOVEL2J=4
A MOTION_TYPE_MOVEL2J=4 value is added to external_controll to determine the branch. The operation is the same as adding a movej. An inverse_q=get_inverse_kin(q) function is added in front of movej.
#Movel point
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL:
acceleration = raw_point[13] / MULT_jointstate
velocity = raw_point[7] / MULT_jointstate
movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius)

    # reset old acceleration
    spline_qdd = [0, 0, 0, 0, 0, 0]
    spline_qd = [0, 0, 0, 0, 0, 0]
  # MoveL2J point
  elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEL2J:
    acceleration = raw_point[13] / MULT_jointstate
    velocity = raw_point[7] / MULT_jointstate
    inverse_q=get_inverse_kin(q)
    movej(inverse_q, a = acceleration, v = velocity, t = tmptime, r = blend_radius)

motion_primitives.h:
enum class MotionType : uint8_t
{
MOVEJ = 0,
MOVEL = 1,
MOVEP = 2,
MOVEC = 3,
MOVEL2J= 4,//self add
SPLINE = 51,
UNKNOWN = 255
};
//self add ---------------------
struct MoveL2JPrimitive : public MotionPrimitive
{
MoveL2JPrimitive(const urcl::Pose& target, const double blend_radius = 0,
const std::chrono::duration duration = std::chrono::milliseconds(0),
const double acceleration = 1.4, const double velocity = 1.04)
{
type = MotionType::MOVEL2J;
target_pose = target;
this->duration = duration;
this->acceleration = acceleration;
this->velocity = velocity;
this->blend_radius = blend_radius;
}

urcl::Pose target_pose;
};

trajectory_point_interface.cpp:
switch (primitive->type)
{
case MotionType::MOVEJ:
{
auto movej_primitive = std::static_pointer_castcontrol::MoveJPrimitive(primitive);
first_block = movej_primitive->target_joint_configuration;
second_block.fill(primitive->velocity);
third_block.fill(primitive->acceleration);
break;
}
case MotionType::MOVEL2J:
{
auto movel_primitive = std::static_pointer_castcontrol::MoveLPrimitive(primitive);
first_block = {
movel_primitive->target_pose.x, movel_primitive->target_pose.y, movel_primitive->target_pose.z,
movel_primitive->target_pose.rx, movel_primitive->target_pose.ry, movel_primitive->target_pose.rz
};
second_block.fill(primitive->velocity);
third_block.fill(primitive->acceleration);
break;
}
instruction_executor.cpp
bool urcl::InstructionExecutor::moveL2J(const urcl::Pose& target, const double acceleration, const double velocity,
const double time, const double blend_radius)
{
return executeMotion({ std::make_sharedcontrol::MoveL2JPrimitive(
target, blend_radius, std::chrono::milliseconds(static_cast(time * 1000)), acceleration, velocity) });
}

[hestrro]

After the modification, I tested it on the real robotic arm ur5 and the pose can be reached correctly. However, since I only gave get_inverse_kin(x, qnear, maxPositionError =1e-
10, maxOrientationError =1e-10, tcp=’active_tcp’) is a parameter that does not have a suitable value for qnear, so it defaults to movej according to the shortest path. This may cause the winding wire outside the robot arm to strangle the robot arm, thus triggering the safety protection of the robot arm. I don’t know how to solve it.