Temporarily changing of joint limits in RobotState

asked 2016-12-02 02:04:41 -0600

Florian_W
15 ●4 ●5 ●8

I am calculating the IK for my robot setup using:

 robot_state::RobotState kinematicStateArm =  *m_arm.getCurrentState();
 foundIkForArm = kinematicStateArm.setFromIK(jointModelGroupArm, targetPose, 10, 0.05);
 kinematicStateArm.copyJointGroupPositions(jointModelGroupArm, jointValuesArm);

Now on certain occasions, I do have to restrict the joint limits in one of the joints further than required for the default case.

Is there any method that allows one to change the joint limits used within the RobotState IK framework? I have seen that the

 moveit::core::JointModel   setVariableBounds()

Reference

allows one to modify the joint limits. However I do see no way of accessing this method from the RobotState API. Does anyone know of a method or way on how to approach my issue?

edit retag flag offensive close merge delete

add a comment

0

answered 2016-12-02 09:18:09 -0600

Florian_W
15 ●4 ●5 ●8

I found that by using the

 moveit_msgs::GetPositionIK

service, it is possible to specify individual moveit_msgs::JointConstraint . First tests yield promising results.

edit flag offensive delete link

Comments

Yes, that service contains the same JointConstraint entry as the MotionPlanRequest that @rbbg mentioned. If you're only after single solutions, then this should work. For actual planning, the approach @rbbg describes could be used (but the constraints are the same).

gvdhoorn ( 2016-12-02 09:22:17 -0600 )edit

@gvdhoorn I only require the end-effector to be "in front" of the robot. Without limiting the joints the Ik solution derived by TRAC-IK is some weirdly looking "behind the shoulder" grasp. The planning procedure may actually utilize the full joint space. Are you aware of a more elegant solution ?

Florian_W ( 2016-12-03 08:55:16 -0600 )edit

add a comment

2

answered 2016-12-02 07:55:21 -0600

rbbg
1823 ●6 ●18 ●26

Hi Florian,

This is possible but pretty tricky. The IK solvers are instantiated when MoveIt! starts and the URDF and SRDF (containing the joint limits) are read from the ROS parameter server. As far as I know, there is currently no way to change the limits in the Kinematics plugin, nor is there an (easy) way to instantiate a new plugin with adjusted limits so this is a bit of a dead end.

However, it is possible to supply the IK plugin with an IKCallback function, that is used to test the validity of the solution using a specified function. In the robot state, this is the GroupStateValidityCallbackFn argument of the setFromIK function. (Typedef found here). This allows you to reject all solutions that lie outside of your new joint limits. Note that this will only allow for stricter joint limits rather than wider. As the IK plugin will still have the original joint limits, there is the chance that the IK solver will come up with the same (invalid) solution each time. To somewhat counter this, the solver will be called with a random seed state for all but the first attempt (out of n attempts as specified in your kinematics.yaml).

Quick note: These random seed states are generated by the supplied JointModelGroup, so if you want to ensure that the random seed state are inside your new limits, you will need to create a JointModelGroup based on JointModel objects with the new limits.

Anyhow, this will hopefully provide you with a solution within your new joint limits. If you want to find a plan to this new joint state, you probably also want to satisfy these limits during the path. Using the JointConstraint field of path_constraints from moveit_msgs::MotionPlanRequest, you can specify contraints on the path that should ensure that the planned path stays within your specified limits. I am not sure how these limits are applied, so I can't say much about the effect on planner performance this has.

In any case, this should give you a solution with satisfying stricter joint limits, but as you can see it will take quite some work to get there. If you get this to work, I'd be interested to hear how it performs!

edit flag offensive delete link

add a comment

Temporarily changing of joint limits in RobotState

2 Answers

Comments

Question Tools

Stats

Related questions

Temporarily changing of joint limits in RobotState edit

2 Answers

Comments

Question Tools

Stats

Related questions

Temporarily changing of joint limits in RobotState