Restrict joint limits: how to clone JointModel to update a JointModelGroup?
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 inRobotModel
fromMoveGroupInterface
; iterating through theJointModel
vector, to only change the necessary position bounds viagetVariableBounds
and respective setter; this however seems to require a deep copy of theJointModel
s I need to modify, and I can't find aclone()
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 updatedJointModelGroup
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.
Asked by aPonza on 2019-01-17 03:39:48 UTC
Comments