Path Planning

Warning

The AI Accelerator requires ROS 2 Humble, so PolyScope X 10.12.1 is the latest compatible release. PolyScope X 10.13 and later use ROS 2 Jazzy and are not currently supported.

Collision-free path planning uses ROS 2 service calls to cuMotion on the AI Accelerator. The move helpers plan via /ur_cumotion/plan_motion and execute the returned joint trajectory on the robot.

Typical call order: ark_path_planning_init() → ark_move_path_plan / ark_move_path_plan_js → ark_path_planning_close(). You only need ark_init() if the same program also uses detection, TF, or other general APIs.

Setup

`ark_path_planning_init()`

ark_path_planning_init()

Creates the ROS 2 service client used by the move helpers.

Arguments

None

Returns

void

Errors / failure modes

Later move calls fail if the cuMotion node is not running or the namespace is wrong

ROS

Service: ARK_ROS2Namespace + "/ur_cumotion/plan_motion"
Type: ur_cumotion_msgs/srv/PlanMotion

Call order

Call once before any ark_move_path_plan* calls.

Example

ark_path_planning_init()

`ark_path_planning_close()`

ark_path_planning_close()

Closes the /ur_cumotion/plan_motion client created by ark_path_planning_init().

Arguments

None

Returns

void

Errors / failure modes

Undefined if called without a prior ark_path_planning_init()

ROS

Closes the plan_motion service client

Call order

Call when path planning is no longer needed.

Example

ark_path_planning_close()

Planning and executing a path

`ark_move_path_plan_js(targetName, targetJ, speed_factor, popUpOnFailure)`

ark_move_path_plan_js(targetName, targetJ, speed_factor, popUpOnFailure)

Plans a collision-free move to joint angles and executes the trajectory.

Arguments

targetName (string): Goal name (e.g. waypoint name). Used to cache planning results for repeated calls to the same target.
targetJ (array of 6 numbers): Desired joint angles (radians).
speed_factor (number): Execution speed scale (0 to 1). Applied to trajectory velocities/accelerations and timing.
popUpOnFailure (bool): If True, show a blocking popup when planning fails.

Returns

bool — True if planning and execution succeed; False on service/planning failure

Errors / failure modes

Service call timeout: 30 seconds
If result.success == False: popup that ur_cumotion/plan_motion is unavailable or timed out; returns False
If planner error_code.val != 0: optionally popup with error_code.message; returns False
Sets globals lastUrCumotionErrorCode and lastUrCumotionErrorMesssage on planner reply

ROS

Service: ARK_ROS2Namespace + "/ur_cumotion/plan_motion"
Type: ur_cumotion_msgs/srv/PlanMotion
Request uses current joint positions as start_position, goal_constraints.joint_constraints=targetJ, empty pose_constraint

Call order

ark_path_planning_init() → ark_move_path_plan_js(...).

Example

ark_path_planning_init()
ok = ark_move_path_plan_js("home", [-1.57, -1.57, -1.57, -1.57, 1.57, 0], 0.5, True)
if ok == False:
  textmsg("Planning failed")
end

`ark_move_path_plan(targetName, targetP, speed_factor, popUpOnFailure)`

ark_move_path_plan(targetName, targetP, speed_factor, popUpOnFailure)

Plans a collision-free move to a tool pose and executes the trajectory.

Arguments

targetName (string): Goal name for cache / identification
targetP (pose): Desired TCP pose. The preamble converts this to a flange target via pose_trans(targetP, pose_inv(get_tcp_offset())) before planning.
speed_factor (number): Execution speed scale (0 to 1)
popUpOnFailure (bool): Popup on planning failure if True

Returns

bool — same success/failure semantics as ark_move_path_plan_js

Errors / failure modes

Same service timeout and error handling as ark_move_path_plan_js
Incorrect TCP offset configuration yields a wrong flange goal

ROS

Same /ur_cumotion/plan_motion service
Request uses goal_constraints.pose_constraint (6-vector flange pose) and empty joint_constraints

Call order

ark_path_planning_init() → ark_move_path_plan(...).

Example

ark_path_planning_init()
ok = ark_move_path_plan("pick", p[0.3, -0.2, 0.2, 0, 3.14, 0], 0.4, True)

ROS2 Services

In addition to the URScript commands above, you can call path-planner ROS 2 services directly.

Service Name	Type	Description
set_joint_limits	ur_cumotion_msgs/srv/SetJointLimits	Set the joint limits to be used by the path planner
attach_object	ur_cumotion_msgs/srv/AttachObject	Include/exclude part geometry during path planning
check_joint_states	ur_cumotion_msgs/srv/CheckJointStates	Check whether the given robot joint states are valid, i.e. collision-free and within joint limits. Note that multiple joint states can be checked at the same time
clear_cache	std_srvs/srv/Trigger	Clear the path planning cache

The parameters for each of the services’ request and response are given below.

SetJointLimits

Request

Parameter	Type	Description
joint_names	string[]	The names of the joints for which the limits will be set
lower_limits	float64[]	The lower limits to be set
upper_limits	float64[]	The upper limits to be set

Valid entries for joint_names are "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", and "wrist_3_joint", which represent the Base, Shoulder, Elbow, Wrist 1, Wrist 2, and Wrist 3 joints, respectively. Although the underlying ROS2 service supports setting joint limits individually, due to limitations of struct parsing, it’s recommended that users set lower and upper limits of all 6 joints at once, in which case joint_names can be omitted or empty. An example will be later provided. Response

Parameter	Type	Description
success	bool	True if set_joint_limits call is successful or false otherwise
error_message	string	The error message if unsuccessful

AttachObject

Request

Parameter	Type	Description
attach_object	bool	Whether to attach or detach the object
object_name	string	The object to be attached. If detached, this can be empty
Response
Parameter	Type	Description
—	—	—
success	bool	True if attach_object call is successful or false otherwise

CheckJointStates

Request

Parameter	Type	Description
joint_states	sensor_msgs/JointState[]	The joint states to be checked for validity
Response
Parameter	Type	Description
—	—	—
error_codes	StateValidityErrorCodes[]	The error validities with error codes for all the joint states

StateValidityErrorCodes

int32 val

int32 VALID=1

int32 INVALID_STATE_JOINT_LIMITS=-1
int32 INVALID_STATE_WORLD_COLLISION=-2
int32 INVALID_STATE_SELF_COLLISION=-3
int32 INVALID_STATE_UNKNOWN_ISSUE=-4

There’s no parameter for the std_srvs/srv/Trigger type.

Example Usages

Users can use the built-in ros_service_client_factory URScript command to create a service client by appending ARK_ROS2Namespace + "/ur_cumotion/" to the service name with the provided service type. See examples below.

joint_limit_client = ros_service_client_factory(ARK_ROS2Namespace + "/ur_cumotion/set_joint_limits", "ur_cumotion_msgs/srv/SetJointLimits")
attach_object_client = ros_service_client_factory(ARK_ROS2Namespace + "/ur_cumotion/attach_object", "ur_cumotion_msgs/srv/AttachObject")
check_joint_states_client = ros_service_client_factory(ARK_ROS2Namespace + "/ur_cumotion/check_joint_states", "ur_cumotion_msgs/srv/CheckJointStates")
clear_cache_client = ros_service_client_factory(ARK_ROS2Namespace + "/ur_cumotion/clear_cache", "std_srvs/srv/Trigger")

Afterwards, users can call the service by calling the call method of the service client. The argument in the call method is a struct that populate all the parameters required by the service type.

To clear the path planning cache, call

clear_cache_client.call(struct())

To set joint limits of all six joints to be - and + 360 degrees, we can call

joint_limit_client.call(struct(joint_names=[], lower_limits=[-3.14159, -3.14159, -3.14159, -3.14159, -3.14159, -3.14159], upper_limits=[3.14159, 3.14159, 3.14159, 3.14159, 3.14159, 3.14159]))

To attach a part named part_1, i.e. include the collision spheres of part_1 in path planning calculations, call

attach_object_client.call(struct(attach_object=True, object_name="part_1"))

Note that part_1 must be defined in the collision models. See example documentation for details.

To detach, i.e. remove the collision spheres of the part, call

attach_object_client.call(struct(attach_object=False, object_name=""))

To check whether the joint configuration [1.5, -3.0603, -1.8406, 0.3079, 1.5734, 1.4186] is valid, we can call

q=[1.5, -3.0603, -1.8406, 0.3079, 1.5734, 1.4186]
header = struct(seq=0, stamp=struct(sec=0, nanosec=0), frame_id="")
joint_state = struct(header=header, name=[], position=q, velocity=[], effort=[])
resp = check_joint_states_client.call(struct(joint_states=[joint_state]))
textmsg("error_code = " + to_str(resp.error_codes[0].val))

Path Planning

Setup

ark_path_planning_init()

ark_path_planning_close()

Planning and executing a path

ark_move_path_plan_js(targetName, targetJ, speed_factor, popUpOnFailure)

ark_move_path_plan(targetName, targetP, speed_factor, popUpOnFailure)

ROS2 Services

SetJointLimits

AttachObject

CheckJointStates

Example Usages

`ark_path_planning_init()`

`ark_path_planning_close()`

`ark_move_path_plan_js(targetName, targetJ, speed_factor, popUpOnFailure)`

`ark_move_path_plan(targetName, targetP, speed_factor, popUpOnFailure)`