Restrict joint limits: how to clone JointModel to update a JointModelGroup?

asked 2019-01-17 02:39:48 -0500

aPonza gravatar image

I'm tackling the problem of restricting joint limits at runtime. I have found only one relevant question on the topic, namely #249253. My situation is similar to Florian's, in the sense I get "behind the shoulder" grasps, and limiting the joints in the urdf seems to solve the planning issue. However:

  • I need the change to be at runtime since the robot is used also in its full joint space;
  • I need control on the robot trajectory anyways (to discard some solutions), since limiting the joints doesn't solve a particular place action, which is still sometimes done with the first joint facing the opposite direction to the place location (I figure the callback talked in the answer could help me discard that solution).

From what I gather, I'm required to:

  • create a JointModelGroup: easiest way seems to be getting the components to construct a one from the one found in RobotModel from MoveGroupInterface; iterating through the JointModel vector, to only change the necessary position bounds via getVariableBounds and respective setter; this however seems to require a deep copy of the JointModels I need to modify, and I can't find a clone() method or a way to easily build the object (it has 20 fields, does this have to be done somehow the hard way?);
  • use this overload of setFromIK() (I'm not using Eigen for poses and I don't think I'll need to give the tip) to get a RobotState to give to setJointValueTarget, then plan+execute like always, by giving it the updated JointModelGroup and a callback function which checks if the end pose has the correct direction. How can the callback help me discard the unwanted solutions?

I could really use some help with these two questions, but I'm very open to suggestions regarding the "am I missing anything?" and the "does this make sense or is there any other (nicer) way?" sides.

edit retag flag offensive close merge delete