0
$\begingroup$

I want to control a UR5 using Universal Robots Driver and MoveIt2 using Pilz Industrial Motion Planner (ROS2 Humble). If I plan a path using RViz with e.g. PTP I receive errors which disable successful planning.

Errors at start-up:

[move_group-9] [ERROR] [1706777537.271727051] [move_group]: Parameter 'robot_description_planning.cartesian_limits.max_trans_vel', is not set in the config file.
[move_group-9] [ERROR] [1706777537.271727051] [move_group]: Parameter 'robot_description_planning.cartesian_limits.max_trans_acc', is not set in the config file.
[move_group-9] [ERROR] [1706777537.271727051] [move_group]: Parameter 'robot_description_planning.cartesian_limits.max_trans_dec', is not set in the config file.
[move_group-9] [ERROR] [1706777537.271727051] [move_group]: Parameter 'robot_description_planning.cartesian_limits.max_rot_vel', is not set in the config file.

Error at planning request:

[move_group-9] [ERROR] [1706777811.530897247] [moveit.ros_planning_pipeline]: Exception caught: 'acceleration limit not set for group up_group'

I've done some digging in the code. pilz_industrial_motion_planner uses for some reason a modified and probably deprecated local copy of ros2_controls joint_limits. Before using the current version the print-out for the most strict joint limit was incomplete.

RCLCPP_ERROR(LOGGER, most_strict_limit_.to_string().c_str()):

has position limits: true [-2.99159, 2.99159]
has velocity limits: true [3.14159]

When using the current version of joint_limits:

has position limits: true [-2.99159, 2.99159]
has velocity limits: true [3.14159]
has acceleration limits: false [nan]
has jerk limits: false [nan]
has effort limits: false [nan]
angle wraparound: false

I've got the joint_limits.yaml from UR Description but I added acceleration limits of an arbitrary value and also set has_acceleration:limits: true. The robot URDF is build using these limits. I've got the impression, that ros2_control also uses the limits from this file, but this doesn't seem to be the case. Additionally when I look at the parameters using ros2 param get <node> <param>, parameters of move_group like robot_description_planning.joint_limits.shoulder_pan_joint.max_velocity are not set, even if they got a value if I print them out.

How do I get ros2_control/joint_limits to read and interpret the values from joint_limits.yaml?

How does the node parameters play into that?

$\endgroup$
0

2 Answers 2

1
$\begingroup$

Thanks Anubhav Singh for the help, since the node parameters where now set properly, I was able to identify the problem. JointLimitsAggregator of the Pilz motion planner doesn't collect other limits then positions and velocities on default.

I've added the function updateAccelerationLimitFromJointModel which does basically the same as the other update functions in order to get the accelerations. Basically the Pilz industrial motion planner cannot be run in MoveIt as is and I am stunned, that nobody has noticed it.

void pilz_industrial_motion_planner::JointLimitsAggregator::updateAccelerationLimitFromJointModel(const moveit::core::JointModel* joint_model, JointLimit& joint_limit)
{
 switch (joint_model->getVariableBounds().size())
 {
  case 0:
   RCLCPP_WARN_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName());
   break;
  case 1:
   joint_limit.has_acceleration_limits = joint_model->getVariableBounds()[0].max_acceleration_;
   break;
  default:
   RCLCPP_WARN_STREAM(LOGGER, "Multi-DOF-Joint '" << joint_model->getName() << "' not supported.");
   joint_limit.has_acceleration_limits = true;
   joint_limit.max_acceleration = 0;
   break;
  }
 }

The function gets then used in pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(const rclcppp::Node::SharedPtr& node, const std::string& param_namespace, const std::vector<const moveit::core::JointModel*>& joint_models) in the same vain as the other update functions.

Unfortunately, no boundary check can be performed yet, since that would require additional implementation of a helper function in moveit_core. But that would go beyond the scope of this question.

$\endgroup$
0
$\begingroup$

You can specify Cartesian velocity and acceleration limits in a file named pilz_cartesian_limits.yaml in your *_moveit_config package as below:

cartesian_limits:
  max_trans_vel: 1
  max_trans_acc: 2.25
  max_trans_dec: -5
  max_rot_vel: 1.57

Have you tried the instructions given here? There is also a section on how to use the planner (see this).

Also, check panda implementation of Pilz Motion Planner for ROS 2 (here).

$\endgroup$
5
  • $\begingroup$ Thank you for this suggestion. I have created such a file within my ur_moveit_config directory. However it is not interacted with ( I would assume this fixes the Cartesian errors). Furthermore, regarding the other error: It seems that my joint limits are only taken from the URDF! The exact values appear in the URDF and there is also no acceleration defined. $\endgroup$
    – NotARobot
    Feb 1 at 14:46
  • $\begingroup$ This source is unfortunately too high level/in-precise. How is joint_limits.yaml loaded, or rather supposed to get loaded? $\endgroup$
    – NotARobot
    Feb 1 at 14:49
  • $\begingroup$ Hey, look at this file. I worked on ur5 and ROS 2 humble quite some time ago. I don't know if this repo still works. I haven't tested this recently. But you can get the idea on how to set joint_limits.yaml. If everything works, please accept the solution. $\endgroup$ Feb 1 at 19:57
  • $\begingroup$ Thank you! The usage of the line robot_description_planning = {"robot_description_planning":load_yaml_abs(str(joint_limit_params.perform(context)))} and adding it to the node parameters of move_group solved, that the parameters appear now correctly in the node parameters. Unfortunately they are not used in the joint_limits class of the Pilz motion planner (an child class of ros2_controls joint_limits. So they are therefore not used in the subsequent look-up for the strictest joint limits inside the planner. $\endgroup$
    – NotARobot
    Feb 2 at 9:38
  • $\begingroup$ See this panda implementation for Pilz motion planner. I updated this in my answer. I hope this solves your query. $\endgroup$ Feb 2 at 11:19

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.