search

ros-industrial / stomp_ros

ROS packages for the STOMP planner (split out of industrial_moveit)
Apache License 2.0
37 stars 27 forks source link

Unable to retrieve the goal from the MotionPlanRequest #34

Open captain-yoshi opened 3 years ago

captain-yoshi commented 3 years ago

When a cartesian constraint goal is sent to STOMP via MoveIt, the target_frame is equal to world (see this line) and thus fails because the world frame is not part of the robot state. This behavior occurs with the panda_moveit_config from moveit_resources package because it uses a floating virtual joint. When a virtual joint is not a fixed frame, the model frame becomes the parent of the virtual joint.

If I change this line with this below it works normally:

//target_frame = model->getModelFrame();
target_frame = model->getRootLinkName();
v4hn commented 3 years ago

Your proposed patch coincides with how several plugins identify the frame name.

Are you sure this also works when the virtual joint actually moves (and thus world and root link do not coincide anymore)? The IKSolver has additional logic for a tf_base_to_root_ transform, but I would not expect it to work as expected without testing.

Btw. looks like the logic here actually relies on frame information from a default-initialized RobotState which is not sufficient for constraints specified relative to the current pose and I would expect robot_state_ here instead. As I don't use stomp in any of my setups at the moment, I can't verify this though.