Ask Your Question
0

MoveIt RobotState initialisation

asked 2017-10-31 13:28:11 -0600

linshenzhen gravatar image

In the MoveIt tutorial for the kinematic part, the sample code does the following to get the robot joint position:

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const robot_state::JointModelGroup *joint_model_group = kinematic_model->getJointModelGroup("right_arm");
const std::vector<std::string> &joint_names = joint_model_group->getVariableNames();
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);

The full code is available from link. However, this doesn't give the current robot joint position. If I print out the position using RobotState::printStateInfo, the position array is always the same no matter how the robot joint positions are. I also tried RobotState::update(true), but it is the same.

I wonder if the robot state requires any initialisation?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2017-11-02 13:48:22 -0600

rivertam gravatar image

The "robot_description" param is just the description of the robot universally (it's loaded from the URDF). The robot model/state does not update automatically, so you're going to be getting back the default joint positions every time because you're just setting it to the default positions. You need to feed in the joint state using kinematic_state->setVariablePositions(...). How you get the joint positions is going to be dependent on your application, but it sounds like you're probably doing a conventional setup where you may be publishing to "joint_states", so you should subscribe to that topic, read the positions in and feed it to the model. The model won't do it for you automatically.

It's a good thing the state doesn't update automatically because it allows us to do things like setting the state to a hypothetical target and testing for things like collisions before actually moving there.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-10-31 13:28:11 -0600

Seen: 110 times

Last updated: Oct 31 '17