Convert CurrentState to targetpose

[Ooijenvangilbert]

In the open class 175 we pick up the robot current state as following :

 // Get Current State
  moveit::core::RobotStatePtr current_state_arm = move_group_arm.getCurrentState(10);

then we set the joint positions in radians, this is all clear.

but then we go to a ‘pregrasp position’ by setting up a Pose. we enter new values here.

The question is, how can i get a Pose from the getCurrentState command? I want to be able to get the current position from the robot as a Pose so i can work from there.

Is this possible?

Thanks in advance

[jtrubatch]

It should be something like:

move_group_arm.getCurrentPose("tool0");  

I believe the move_group_arm object should have access to the correct libraries. If not it would be:

moveit::planning_interface::MoveGroupInterface::getCurrentPose("tool0");

tool0 could be replaced with wrist_3 or whatever that wrist link’s name is.

However, to move to a pose you don’t need the current pose.

[Ooijenvangilbert]

Thanks so much, this really boosted progress here.

The correct link name in my case was ‘wrist_3_link’