int main(int argc, char **argv)
{
ros::init (argc, argv, "right_arm_kinematics");
ros::AsyncSpinner spinner(1);
spinner.start();
/* Load the robot model */
robot_model_loader::RDFLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
/* Get and print the name of the coordinate frame in which the transforms for this model are computed*/
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
/* WORKING WITH THE KINEMATIC STATE */
/* Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
/* Set all joints in this state to their default values */
kinematic_state->setToDefaultValues();
/* Get the configuration for the joints in the right arm of the PR2*/
robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm");
/* Get the names of the joints in the right_arm*/
const std::vector<std::string> &joint_names = joint_state_group->getJointModelGroup()->getJointModelNames();
/* Get the joint states for the right arm*/
std::vector<double> joint_values;
joint_state_group->getVariableValues(joint_values);
/* Print joint names and values */
for(std::size_t i = 0; i < joint_names.size(); ++i)
{
ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
/* Set one joint in the right arm outside its joint limit */
joint_values[0] = 1.57;
joint_state_group->setVariableValues(joint_values);
/* Check whether any joint is outside its joint limits */
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
/* Enforce the joint limits for this state and check again*/
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
/* FORWARD KINEMATICS */
/* Compute FK for a set of random joint values*/
joint_state_group->setToRandomValues();
const Eigen::Affine3d &end_effector_state = joint_state_group->getRobotState()->getLinkState("r_wrist_roll_link")->getGlobalLinkTransform();
/* Print end-effector pose. Remember that this is in the model frame */
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());
/* INVERSE KINEMATICS */
/* Set joint state group to a set of random values*/
joint_state_group->setToRandomValues();
/* Do IK on the pose we just generated using forward kinematics
* Here 10 is the number of random restart and 0.1 is the allowed time after
* each restart
*/
bool found_ik = joint_state_group->setFromIK(end_effector_state, 10, 0.1);
/* Get and print the joint values */
if (found_ik)
{
joint_state_group->getVariableValues(joint_values);
for(std::size_t i=0; i < joint_names.size(); ++i)
{
ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
}
else
{
ROS_INFO("Did not find IK solution");
}
/* DIFFERENTIAL KINEMATICS */
/* Get and print the Jacobian for the right arm*/
Eigen::Vector3d reference_point_position(0.0,0.0,0.0);
Eigen::MatrixXd jacobian;
joint_state_group->getJacobian(joint_state_group->getJointModelGroup()->getLinkModelNames().back(),
reference_point_position,
jacobian);
ROS_INFO_STREAM("Jacobian: " << jacobian);
ros::shutdown();
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "ptu_kinematics");
ros::AsyncSpinner spinner(1);
spinner.start();
// BEGIN_TUTORIAL
// Start
// ^^^^^
// Setting up to start using the RobotModel class is very easy. In
// general, you will find that most higher-level components will
// return a shared pointer to the RobotModel. You should always use
// that when possible. In this example, we will start with such a
// shared pointer and discuss only the basic API. You can have a
// look at the actual code API for these classes to get more
// information about how to use more features provided by these
// classes.
//
// We will start by instantiating a
// `RobotModelLoader`_
// object, which will look up
// the robot description on the ROS parameter server and construct a
// :moveit_core:`RobotModel` for us to use.
//
// .. _RobotModelLoader: http://docs.ros.org/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
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());
// Using the :moveit_core:`RobotModel`, we can construct a
// :moveit_core:`RobotState` that maintains the configuration
// of the robot. We will set all joints in the state to their
// default values. We can then get a
// :moveit_core:`JointModelGroup`, which represents the robot
// model for a particular group, e.g. the "right_arm" of the PR2
// robot.
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("ptu");
const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
// Get Joint Values
// ^^^^^^^^^^^^^^^^
// We can retreive the current set of joint values stored in the state for the right arm.
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for(std::size_t i = 0; i < joint_names.size(); ++i)
{
ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
// Joint Limits
// ^^^^^^^^^^^^
// setJointGroupPositions() does not enforce joint limits by itself, but a call to enforceBounds() will do it.
/* Set one joint in the right arm outside its joint limit */
joint_values[0] = 1.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
/* Check whether any joint is outside its joint limits */
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
/* Enforce the joint limits for this state and check again*/
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
// Forward Kinematics
// ^^^^^^^^^^^^^^^^^^
// Now, we can compute forward kinematics for a set of random joint
// values. Note that we would like to find the pose of the
// "r_wrist_roll_link" which is the most distal link in the
// "right_arm" of the robot.
kinematic_state->setToRandomPositions(joint_model_group);
double pan = 0.0;
double tilt = 0.0;
kinematic_state->setJointPositions("pan", &pan);
kinematic_state->setJointPositions("tilt", &tilt);
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("head_xtion_depth_optical_frame");
/* Print end-effector pose. Remember that this is in the model frame */
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());
// Inverse Kinematics
// ^^^^^^^^^^^^^^^^^^
// We can now solve inverse kinematics (IK) for the right arm of the
// PR2 robot. To solve IK, we will need the following:
// * The desired pose of the end-effector (by default, this is the last link in the "right_arm" chain): end_effector_state that we computed in the step above.
// * The number of attempts to be made at solving IK: 5
// * The timeout for each attempt: 0.1 s
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
// Now, we can print out the IK solution (if found):
if (found_ik)
{
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for(std::size_t i=0; i < joint_names.size(); ++i)
{
ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
}
else
{
ROS_INFO("Did not find IK solution");
}
// Get the Jacobian
// ^^^^^^^^^^^^^^^^
// We can also get the Jacobian from the :moveit_core:`RobotState`.
Eigen::Vector3d reference_point_position(0.0,0.0,0.0);
Eigen::MatrixXd jacobian;
kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
reference_point_position,
jacobian);
ROS_INFO_STREAM("Jacobian: " << jacobian);
// END_TUTORIAL
ros::shutdown();
return 0;
}
