Path Planning
Path planning utilizes ROS2 service calls to the services running on AI Accelerator.
Setup
ark_path_planning_init()
Initializes the ROS2 service client to the path planner running on AI Accelerator
Planning and executing a path
The following is the two main methods to plan a path and execute it.
ark_move_path_plan_js(targetName, targetJ, speed_factor, popUpOnFailure) => bool
Perform a collision free move to a desired set of joint angles at the specified speed factor. Arguments
targetName(string): The name of the target. Can be a waypoint name. It’s used to cache the path planning result to speed up subsequent calls to the same target.targetJ(array of 6 numbers): The desired joint angles.speed_factor(number): The speed factor (from 0 to 1) at which to execute the path.popUpOnFailure(bool): The flag to indicate whether we want the popup dialog to display if path planning fails.
ark_move_path_plan(targetName, targetP, speed_factor, popUpOnFailure) => bool
Perform a collision free move to a desired tool position at the specified speed factor Arguments
targetName(string): The name of the target. Can be a waypoint name. It’s used to cache the path planning result to speed up subsequent calls to the same target.targetP(pose): The desired tool pose.speed_factor(number): The speed factor (from 0 to 1) at which to execute the path.popUpOnFailure(bool): The flag to indicate whether we want the popup dialog to display if path planning fails.
ROS2 Services
In addition to the URScript commands above, users can interact with the path planner via the ROS2 services provided in the following table.
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, Elblow, 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", "std_srvs/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))