//service function for execute_cartesian_ik_trajectory
bool execute_cartesian_ik_trajectory(
ur5_control::ExecuteCartesianIKTrajectory::Request &req,
ur5_control::ExecuteCartesianIKTrajectory::Response &res)
{
int trajectory_length = req.poses.size();
int i, j;
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
/* 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));
/* Get the configuration for the joints in the right arm of the PR2*/
robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("manipulator");
/* Get the names of the joints in the right_arm*/
const std::vector<std::string> &joint_names = joint_state_group->getJointModelGroup()->getJointModelNames();
//IK takes in Cartesian poses stamped with the frame they belong to
geometry_msgs::PoseStamped stamped_pose;
stamped_pose.header = req.header;
stamped_pose.header.stamp = ros::Time::now();
bool success;
std::vector<double *> joint_trajectory;
//get the current joint angles (to find ik solutions close to)
double last_angles[6];
get_current_joint_angles(last_angles);
//find IK solutions for each point along the trajectory
//and stick them into joint_trajectory
for(i=0; i<trajectory_length; i++){
stamped_pose.pose = req.poses[i];
double *trajectory_point = new double[6];
success = run_ik(stamped_pose.pose, last_angles, trajectory_point, "ee_link",
joint_state_group, joint_names);
joint_trajectory.push_back(trajectory_point);
if(!success){
ROS_ERROR("IK solution not found for trajectory point number %d!\n", i);
return 0;
}
for(j=0; j<6; j++) last_angles[j] = trajectory_point[j];
}
//run the resulting joint trajectory
ROS_INFO("executing joint trajectory");
success = execute_joint_trajectory(joint_trajectory);
res.success = success;
return success;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ik");
ros::NodeHandle n;
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();
geometry_msgs::Point pos;
pos.x = 0.5;
pos.y = 0;
pos.z = 0.5;
geometry_msgs::Quaternion qua;
qua.x = 0;
qua.y = 0;
qua.z = 0;
qua.w = 1;
geometry_msgs::Pose end_effector_state;
end_effector_state.position = pos;
end_effector_state.orientation = qua;
std::vector<double> joint_values;
std::vector<std::string> joint_names;
joint_names.push_back("joint_1");
joint_names.push_back("joint_2");
joint_names.push_back("joint_3");
joint_names.push_back("joint_4");
joint_names.push_back("joint_5");
joint_names.push_back("joint_6");
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("manipulator");
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
if(found_ik)
{
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
ROS_INFO("asd\n");
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");
}
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "state_display_tutorial");
ros::NodeHandle nh;
/* Needed for ROS_INFO commands to work */
ros::AsyncSpinner spinner(1);
spinner.start();
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
/* 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));
/* Get the configuration for the joints in the right arm of the PR2*/
const robot_model::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("base");
const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
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]);
}
ros::Publisher robot_state_publisher = nh.advertise<pr2_moveit_ltl::baseState>( "base_robot_state", 1 );
ros::Rate loop_rate(1);
while(ros::ok()){
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
pr2_moveit_ltl::baseState msg;
msg.joints = joint_values;
msg.names = joint_names;
robot_state_publisher.publish( msg );
joint_values[0] = joint_values[0] + 0.2;
kinematic_state->setJointGroupPositions("base", joint_values);
ros::spinOnce();
loop_rate.sleep();
}
/* loop at 1 Hz */
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "state_display_tutorial");
/* Needed for ROS_INFO commands to work */
ros::AsyncSpinner spinner(1);
spinner.start();
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
/* 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));
const robot_model::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("base");
ros::NodeHandle nh;
ros::Publisher robot_state_publisher = nh.advertise<moveit_msgs::DisplayRobotState>( "tutorial_robot_state", 1 );
/* loop at 1 Hz */
ros::Rate loop_rate(1);
for (int cnt=0; cnt<5 && ros::ok(); cnt++)
{
kinematic_state->setToRandomPositions(joint_model_group);
/* get a robot state message describing the pose in kinematic_state */
moveit_msgs::DisplayRobotState msg;
robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state);
/* send the message to the RobotState display */
std::cout << "Num:" << cnt << std::endl;
robot_state_publisher.publish( msg );
/* let ROS send the message, then wait a while */
ros::spinOnce();
loop_rate.sleep();
}
ros::shutdown();
return 0;
}
ARM_manager::ARM_manager()
{
// Load the robot model
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
my_robot_model_loader = robot_model_loader;
// Get a shared pointer to the model
my_kinematic_model = my_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", my_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(my_kinematic_model));
my_kinematic_state = kinematic_state;
// Set all joints in this state to their default values
my_kinematic_state->setToDefaultValues();
RA_joint_model_group = my_kinematic_model->getJointModelGroup("right_arm");
RA_group_ = my_kinematic_state->getJointStateGroup("right_arm");
// Get the names of the joints in the right_arm
RA_joint_names = RA_joint_model_group->getJointModelNames();
LA_joint_model_group = my_kinematic_model->getJointModelGroup("left_arm");
LA_group_ = my_kinematic_state->getJointStateGroup("left_arm");
// Get the names of the joints in the left_arm
LA_joint_names = LA_joint_model_group->getJointModelNames();
action_sub_ = nh.subscribe < std_msgs::Int32 > ("/ROBOT/action", 5, &ARM_manager::actionCallback, this);
firepose_sub_ = nh.subscribe < geometry_msgs::PoseStamped > ("/ROBOT/firepose", 5, &ARM_manager::fireposeCallback, this); // inclure la couleur dans le twist
fruitcolor_sub_ = nh.subscribe < std_msgs::Int32 > ("/ROBOT/fruitcolor", 5, &ARM_manager::fruitcolorCallback, this);
start_game_sub_ = nh.subscribe < std_msgs::Empty > ("/GENERAL/start_game", 5, &ARM_manager::startgameCallback, this);
Rdebug_sub_ = nh.subscribe < geometry_msgs::PoseStamped > ("/DEBUG/right_arm_pose", 5, &ARM_manager::RdebugCallback, this); // inclure la couleur dans le twist
Ldebug_sub_ = nh.subscribe < geometry_msgs::PoseStamped > ("/DEBUG/left_arm_pose", 5, &ARM_manager::LdebugCallback, this); // inclure la couleur dans le twist
alpha_pub = nh.advertise < std_msgs::Int32 > ("/ROBOT/alpha_ros", 5);
delta_pub = nh.advertise < std_msgs::Int32 > ("/ROBOT/delta_ros", 5);
grip_pub = nh.advertise < std_msgs::Int8 > ("/ROBOT/grip", 5);
rpump_pub = nh.advertise < std_msgs::Int8 > ("pump_ros", 5);
lpump_pub = nh.advertise < std_msgs::Int8 > ("/ROBOT/lpump", 5);
done_pub = nh.advertise < std_msgs::Empty > ("/ROBOT/done", 5);
Rjoint1_pub = nh.advertise < std_msgs::Float64 > ("/Rlink1_controller/command", 5);
Rjoint2_pub = nh.advertise < std_msgs::Float64 > ("/Rlink2_controller/command", 5);
Rjoint3_pub = nh.advertise < std_msgs::Float64 > ("/Rlink3_controller/command", 5);
Rjoint4_pub = nh.advertise < std_msgs::Float64 > ("/Rlink4_controller/command", 5);
Rjoint5_pub = nh.advertise < std_msgs::Float64 > ("/Rlink5_controller/command", 5);
Ljoint1_pub = nh.advertise < std_msgs::Float64 > ("/Llink1_controller/command", 5);
Ljoint2_pub = nh.advertise < std_msgs::Float64 > ("/Llink2_controller/command", 5);
Ljoint3_pub = nh.advertise < std_msgs::Float64 > ("/Llink3_controller/command", 5);
Ljoint4_pub = nh.advertise < std_msgs::Float64 > ("/Llink4_controller/command", 5);
Ljoint5_pub = nh.advertise < std_msgs::Float64 > ("/Llink5_controller/command", 5);
speed_Rjoint1 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink1_controller/set_speed", true);
speed_Rjoint2 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink2_controller/set_speed", true);
speed_Rjoint3 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink3_controller/set_speed", true);
speed_Rjoint4 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink4_controller/set_speed", true);
speed_Rjoint5 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink5_controller/set_speed", true);
slope_Rjoint1 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink1_controller/set_compliance_slope", true);
slope_Rjoint2 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink2_controller/set_compliance_slope", true);
slope_Rjoint3 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink3_controller/set_compliance_slope", true);
slope_Rjoint4 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink4_controller/set_compliance_slope", true);
slope_Rjoint5 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink5_controller/set_compliance_slope", true);
speed_Ljoint1 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink1_controller/set_speed", true);
speed_Ljoint2 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink2_controller/set_speed", true);
speed_Ljoint3 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink3_controller/set_speed", true);
speed_Ljoint4 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink4_controller/set_speed", true);
speed_Ljoint5 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink5_controller/set_speed", true);
slope_Ljoint1 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink1_controller/set_compliance_slope", true);
slope_Ljoint2 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink2_controller/set_compliance_slope", true);
slope_Ljoint3 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink3_controller/set_compliance_slope", true);
slope_Ljoint4 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink4_controller/set_compliance_slope", true);
slope_Ljoint5 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink5_controller/set_compliance_slope", true);
prev_Rjoint1 = 0.0;
prev_Rjoint2 = 0.0;
prev_Rjoint3 = 0.0;
prev_Rjoint4 = 0.0;
prev_Rjoint5 = 0.0;
max_speed = 5.0;
usleep(1000000);
dynamixel_controllers::SetComplianceSlope tmp_slope;
tmp_slope.request.slope = 90;
if (slope_Rjoint1.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Rjoint2.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Rjoint3.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Rjoint4.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Rjoint5.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Ljoint1.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Ljoint2.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Ljoint3.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Ljoint4.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
if (slope_Ljoint5.call(tmp_slope))
{
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service SetSlope");
}
usleep(500000);
init_pose();
//standard_pose();
}
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;
}
TeleopMK2::TeleopMK2()
{
joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/command", 5);
joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopMK2::joyCallback, this);
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
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 MK2 */
const robot_state::JointModelGroup* joint_state_group = kinematic_state->getJointModelGroup("manipulator");//"manipulator");
/* Get the names of the joints in the right_arm*/
const std::vector<std::string> &joint_names = joint_state_group->getJointModelNames();
// Get Joint Values
// ^^^^^^^^^^^^^^^^
/* Get the joint states for the right arm*/
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_state_group,joint_values);
std::vector<double> vel;
double p[] = {0.4,0.4,0.4,0.4,0.6,0.6,0.6};
std::vector<double> a(p, p+7);
sensor_msgs::JointState msg;
ros::Rate r(50);
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("link_7");
Eigen::Affine3d new_end_effector_state;
tf::poseEigenToMsg(end_effector_state, actual_end_effector_);
while(nh_.ok())
{
tf::poseMsgToEigen(actual_end_effector_, new_end_effector_state);
bool found_ik = kinematic_state->setFromIK(joint_state_group, new_end_effector_state, 10, 0.1);
if (found_ik)
{
kinematic_state->copyJointGroupPositions(joint_state_group,joint_values);
msg.header.stamp = ros::Time::now();
msg.name = joint_names;
msg.position = joint_values;
msg.velocity = a;
joint_pub_.publish(msg);
}
else
{
ROS_INFO("Did not find IK solution");
}
ros::spinOnce();
r.sleep();
}
}
void TeleopMK2::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
if (joy->buttons[0])
{
//TeleopMK2::kinematic_state->setToDefaultValues();
actual_end_effector_.position.x = 0.29855;
actual_end_effector_.position.y = -0.24952;
actual_end_effector_.position.z = -0.88;
}
else if (joy->buttons[10])
{// pick up box UP pose Pos: 0.29855 -0.0208635 -0.37089 | 0.29855 -0.0208635 -0.552698
//actual_end_effector_.position.x = 0.3;
//actual_end_effector_.position.y = 0.0;
//actual_end_effector_.position.z = -0.88;
actual_end_effector_.position.x = 0.29;
actual_end_effector_.position.y = -0.02;
actual_end_effector_.position.z = -0.42;
}
else if (joy->buttons[8])
{// Pick up box down pose Pos: Pos: 0.43464 -0.0611224 -0.689392
//actual_end_effector_.position.x = 0.3;
//actual_end_effector_.position.y = -0.25;
//actual_end_effector_.position.z = -0.75;
actual_end_effector_.position.x = 0.43;
actual_end_effector_.position.y = -0.061;
actual_end_effector_.position.z = -0.688;
}
else if (joy->buttons[6])
{// put down box DOWN pose Pos: 0.285059 -0.506483 -0.762183
//actual_end_effector_.position.x = 0.3;
//actual_end_effector_.position.y = -0.5;
//actual_end_effector_.position.z = -0.88;
actual_end_effector_.position.x = 0.285;
actual_end_effector_.position.y = -0.506;
actual_end_effector_.position.z = -0.762;
}
else if (joy->buttons[11])
{///put down box UP pose : Pos: 0.285059 -0.506483 -0.451006
actual_end_effector_.position.x =0.285;
actual_end_effector_.position.y = -0.506;
actual_end_effector_.position.z =-0.451;
}
else if (joy->buttons[9])
{
//actual_end_effector_.position.x =
//actual_end_effector_.position.y =
//actual_end_effector_.position.z =
}
else if (joy->buttons[7])
{
//actual_end_effector_.position.x =
//actual_end_effector_.position.y =
//actual_end_effector_.position.z =
}
actual_end_effector_.position.x += 0.005 * joy->axes[1];
actual_end_effector_.position.y += 0.005 * joy->axes[0];
actual_end_effector_.position.z += 0.005 * joy->axes[2]; //-0.88 + 0.3 * joy->axes[3];
actual_end_effector_.orientation.w = 1.0;
actual_end_effector_.orientation.x = 0.0;
actual_end_effector_.orientation.y = 0.0;
actual_end_effector_.orientation.z = 0.0;
ROS_INFO_STREAM ("Pos: " << actual_end_effector_.position.x << " " << actual_end_effector_.position.y << " " << actual_end_effector_.position.z);
if (joy->buttons[0])
{
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
}
}
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;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "right_arm_kinematics");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle;
// Start a service client
ros::ServiceClient service_client = node_handle.serviceClient<moveit_msgs::GetPositionIK> ("compute_ik");
ros::Publisher robot_state_publisher = node_handle.advertise<moveit_msgs::DisplayRobotState>( "tutorial_robot_state", 1 );
while(!service_client.exists())
{
ROS_INFO("Waiting for service");
sleep(1.0);
}
moveit_msgs::GetPositionIK::Request service_request;
moveit_msgs::GetPositionIK::Response service_response;
service_request.ik_request.group_name = "left_arm";
service_request.ik_request.pose_stamped.header.frame_id = "torso_lift_link";
service_request.ik_request.pose_stamped.pose.position.x = 0.75;
service_request.ik_request.pose_stamped.pose.position.y = 0.188;
service_request.ik_request.pose_stamped.pose.position.z = 0.0;
service_request.ik_request.pose_stamped.pose.orientation.x = 0.0;
service_request.ik_request.pose_stamped.pose.orientation.y = 0.0;
service_request.ik_request.pose_stamped.pose.orientation.z = 0.0;
service_request.ik_request.pose_stamped.pose.orientation.w = 1.0;
/* Call the service */
service_client.call(service_request, service_response);
ROS_INFO_STREAM("Result: " << ((service_response.error_code.val == service_response.error_code.SUCCESS) ? "True " : "False ") << service_response.error_code.val);
/* Filling in a seed state */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("left_arm");
/* Get the names of the joints in the right_arm*/
service_request.ik_request.robot_state.joint_state.name = joint_state_group->getJointModelGroup()->getJointModelNames();
/* Get the joint values and put them into the message, this is where you could put in your own set of values as well.*/
joint_state_group->setToRandomValues();
joint_state_group->getVariableValues(service_request.ik_request.robot_state.joint_state.position);
/* Call the service again*/
service_client.call(service_request, service_response);
ROS_INFO_STREAM("Result: " << ((service_response.error_code.val == service_response.error_code.SUCCESS) ? "True " : "False ") << service_response.error_code.val);
/* Check for collisions*/
service_request.ik_request.avoid_collisions = true;
/* Call the service again*/
service_client.call(service_request, service_response);
ROS_INFO_STREAM("Result: " << ((service_response.error_code.val == service_response.error_code.SUCCESS) ? "True " : "False ") << service_response.error_code.val);
/* Visualize the result*/
moveit_msgs::DisplayRobotState msg;
joint_state_group->setVariableValues(service_response.solution.joint_state);
robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state);
robot_state_publisher.publish(msg);
//Sleep to let the message go through
ros::Duration(2.0).sleep();
ros::shutdown();
return 0;
}
bool StompOptimizationTask::setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& scene,
const moveit_msgs::MotionPlanRequest& request)
{
planning_scene_ = scene;
feature_set_->setPlanningScene(planning_scene_);
motion_plan_request_ = &request;
control_cost_weight_ = 0.0;
last_executed_rollout_ = -1;
reference_frame_ = kinematic_model_->getModelFrame();
dt_ = movement_duration_ / (num_time_steps_-1.0);
num_time_steps_all_ = num_time_steps_ + 2*stomp::TRAJECTORY_PADDING;
if (!kinematic_model_->hasJointModelGroup(planning_group_name_))
{
ROS_ERROR("STOMP: Planning group %s doesn't exist!", planning_group_name_.c_str());
return false;
}
joint_model_group_ = kinematic_model_->getJointModelGroup(planning_group_name_);
num_dimensions_ = joint_model_group_->getVariableCount();
// get the start and goal positions from the message
kinematic_state::KinematicState kinematic_state(kinematic_model_);
kinematic_state.setStateValues(request.start_state.joint_state);
kinematic_state.getJointStateGroup(planning_group_name_)->getVariableValues(start_joints_);
std::map<std::string, double> goal_joint_map;
for (size_t i=0; i<request.goal_constraints[0].joint_constraints.size(); ++i)
{
goal_joint_map[request.goal_constraints[0].joint_constraints[i].joint_name] =
request.goal_constraints[0].joint_constraints[i].position;
}
kinematic_state.setStateValues(goal_joint_map);
kinematic_state.getJointStateGroup(planning_group_name_)->getVariableValues(goal_joints_);
// create the derivative costs
std::vector<Eigen::MatrixXd> derivative_costs(num_dimensions_,
Eigen::MatrixXd::Zero(num_time_steps_all_, stomp::NUM_DIFF_RULES));
std::vector<Eigen::VectorXd> initial_trajectory(num_dimensions_,
Eigen::VectorXd::Zero(num_time_steps_all_));
for (int d=0; d<num_dimensions_; ++d)
{
derivative_costs[d].col(stomp::STOMP_ACCELERATION) = Eigen::VectorXd::Ones(num_time_steps_all_);
initial_trajectory[d] = Eigen::VectorXd::Zero(num_time_steps_all_);
initial_trajectory[d].head(stomp::TRAJECTORY_PADDING) = Eigen::VectorXd::Ones(stomp::TRAJECTORY_PADDING) * start_joints_[d];
initial_trajectory[d].tail(stomp::TRAJECTORY_PADDING) = Eigen::VectorXd::Ones(stomp::TRAJECTORY_PADDING) * goal_joints_[d];
}
policy_.reset(new stomp::CovariantMovementPrimitive());
policy_->initialize(num_time_steps_,
num_dimensions_,
movement_duration_,
derivative_costs,
initial_trajectory);
policy_->setToMinControlCost();
std::vector<Eigen::VectorXd> params_all;
policy_->getParametersAll(params_all);
// initialize all trajectories
trajectories_.resize(num_rollouts_+1);
for (int i=0; i<num_rollouts_+1; ++i)
{
trajectories_[i].reset(new StompTrajectory(num_time_steps_all_, kinematic_model_, planning_group_name_, policy_));
trajectories_[i]->features_ = Eigen::MatrixXd(num_time_steps_all_, num_split_features_);
trajectories_[i]->weighted_features_ = Eigen::MatrixXd(num_time_steps_all_, num_split_features_);
trajectories_[i]->costs_ = Eigen::VectorXd(num_time_steps_all_);
trajectories_[i]->gradients_.resize(num_split_features_, Eigen::MatrixXd::Zero(num_dimensions_, num_time_steps_all_));
trajectories_[i]->validities_.resize(num_time_steps_all_, 1);
trajectories_[i]->setJointPositions(params_all, 0);
}
// set up feature basis functions
std::vector<double> feature_split_magnitudes(num_feature_basis_functions_);
feature_basis_functions_ = Eigen::MatrixXd::Zero(num_time_steps_, num_feature_basis_functions_);
for (int t=0; t<num_time_steps_; ++t)
{
// compute feature splits and normalizations
double p = double(t) / double(num_time_steps_-1);
double sum = 0.0;
for (int i=0; i<num_feature_basis_functions_; ++i)
{
feature_split_magnitudes[i] = (1.0 / (feature_basis_stddev_[i] * sqrt(2.0*M_PI))) *
exp(-pow((p - feature_basis_centers_[i])/feature_basis_stddev_[i],2)/2.0);
sum += feature_split_magnitudes[i];
}
for (int i=0; i<num_feature_basis_functions_; ++i)
{
feature_split_magnitudes[i] /= sum;
feature_basis_functions_(t, i) = feature_split_magnitudes[i];
}
}
//policy_->writeToFile(std::string("/tmp/test.txt"));
if (publish_distance_fields_)
{
// ping the distance field once to initialize it so that we can publish
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
collision_request.group_name = planning_group_name_;
collision_request.contacts = true;
collision_request.max_contacts = 100;
boost::shared_ptr<collision_detection::GroupStateRepresentation> gsr;
collision_world_df_->checkCollision(collision_request, collision_result, *collision_robot_df_,
kinematic_state, planning_scene_->getAllowedCollisionMatrix(),
gsr);
// this is the goal state, there should be no collisions
if (collision_result.collision)
{
ROS_ERROR("STOMP: goal state in collision!");
collision_detection::CollisionResult::ContactMap::iterator it;
for (it = collision_result.contacts.begin(); it!= collision_result.contacts.end(); ++it)
{
ROS_ERROR("Collision between %s and %s", it->first.first.c_str(), it->first.second.c_str());
}
}
boost::shared_ptr<const distance_field::DistanceField> robot_df = collision_robot_df_->getLastDistanceFieldEntry()->distance_field_;
boost::shared_ptr<const distance_field::DistanceField> world_df = collision_world_df_->getDistanceField();
visualization_msgs::Marker robot_df_marker, world_df_marker;
robot_df->getIsoSurfaceMarkers(0.0, 0.03, reference_frame_, ros::Time::now(), Eigen::Affine3d::Identity(), robot_df_marker);
world_df->getIsoSurfaceMarkers(0.0, 0.03, reference_frame_, ros::Time::now(), Eigen::Affine3d::Identity(), world_df_marker);
robot_df_marker.ns="robot_distance_field";
world_df_marker.ns="world_distance_field";
viz_distance_field_pub_.publish(robot_df_marker);
viz_distance_field_pub_.publish(world_df_marker);
// visualize the robot model too
visualization_msgs::MarkerArray body_marker_array;
getBodySphereVisualizationMarkers(gsr, reference_frame_, ros::Time::now(), body_marker_array);
viz_robot_body_pub_.publish(body_marker_array);
}
return true;
}
int main(int argc, char **argv)
{
// Iniciar nodo IK
ros::init (argc, argv, "kuka_ik");
ros::NodeHandle n;
ros::NodeHandle nh("~");
// Parametro rate
int rate;
nh.param("rate", rate, 5);
ros::Rate loop_rate(rate);
ROS_INFO("IK Solve: %d Hz", rate);
// Parametro origin
std::string origin;
nh.param<std::string>("origin", origin, "world");
// Cargar robot KUKA
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
// Frame por defecto base
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
// Grupo de movimiento del robot
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("arm");
// Obtener nombres de joints
const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
// Posicion actual de joints
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
// Publisher for KUKA model joint position from IK solver
ros::Publisher kukaJointPub = n.advertise<sensor_msgs::JointState>("joint_states", 1000);
// Suscriber for goal position (InteractiveMarker)
ros::Subscriber kukaIkGoalSub = n.subscribe("goal_pose", 50, kukaIkCallback);
// JointState Msg for KUKA model from IK solver
sensor_msgs::JointState jointMsg;
jointMsg.name.resize(6);
jointMsg.position.resize(6);
jointMsg.velocity.resize(6);
for(std::size_t i=0; i < joint_names.size(); ++i){
jointMsg.name[i] = joint_names[i].c_str();
jointMsg.position[i] = joint_values[i];
}
// Publish init joint state
ros::Duration(1).sleep();
jointMsg.header.stamp = ros::Time::now();
kukaJointPub.publish(jointMsg);
// Mensaje
while (ros::ok()) {
if (updatePose){
// IK
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
bool found_ik = kinematic_state->setFromIK(joint_model_group, goalPose, 5, 0.01);
// IK solution (if found):
if (found_ik) {
// Actualizar joint_values
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
// for(std::size_t i=0; i < joint_names.size(); ++i){
// jointMsg.position[i] = joint_values[i];
// //ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
// }
jointMsg.position[0] = joint_values[0];
jointMsg.position[1] = joint_values[1];
jointMsg.position[2] = joint_values[2];
//jointMsg.position[3] = 0.0; //GIRO KINECT
jointMsg.position[3] = joint_values[3]; //GIRO KINECT
jointMsg.position[4] = joint_values[4];
jointMsg.position[5] = joint_values[5];
}
else {
ROS_ERROR_THROTTLE(2,"INVERSE KINEMATIC IS NOT FEASIBLE!");
}
updatePose=false;
}
// Publicar mensaje para KUKA model
jointMsg.header.stamp = ros::Time::now();
kukaJointPub.publish(jointMsg);
ros::spinOnce();
loop_rate.sleep();
}
ros::shutdown();
return 0;
}
int main(int argc, char **argv)
{
// Iniciar nodo IK
ros::init (argc, argv, "kuka_ik");
ros::NodeHandle n;
ros::NodeHandle nh("~");
// Parametro rate
int rate;
nh.param("rate", rate, 5);
ros::Rate loop_rate(rate);
ROS_INFO("IK Solve: %d Hz", rate);
// Parametro origin
std::string origin;
nh.param<std::string>("origin", origin, "world");
// Cargar robot KUKA
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
// Frame por defecto base
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
// Grupo de movimiento del robot
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("arm");
// Obtener nombres de joints
const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
// Posicion actual de joints
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
// Publisher para posiciones KUKA Shadow
ros::Publisher kukaShadowPub = n.advertise<sensor_msgs::JointState>("joint_states", 1000);
// Suscriber para posiciones objetivo para KUKA Shadow
ros::Subscriber kukaShadowSub = n.subscribe("efector_pose", 50, kukaEfectorCallback);
// Mensaje para KUKA Shadow
sensor_msgs::JointState shadowJointMsg;
shadowJointMsg.name.resize(6);
shadowJointMsg.position.resize(6);
shadowJointMsg.velocity.resize(6);
for(std::size_t i=0; i < joint_names.size(); ++i){
shadowJointMsg.name[i] = joint_names[i].c_str();
shadowJointMsg.position[i] = joint_values[i];
}
// Mensaje
while (ros::ok()) {
// IK
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
bool found_ik = kinematic_state->setFromIK(joint_model_group, goalPose, 5, 0.01);
// Now, we can print out the IK solution (if found):
if (found_ik) {
// Actualizar joint_values
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
// Publicar mensaje para KUKA Shadow
shadowJointMsg.header.stamp = ros::Time::now();
for(std::size_t i=0; i < joint_names.size(); ++i){
shadowJointMsg.name[i] = joint_names[i].c_str();
shadowJointMsg.position[i] = joint_values[i];
}
kukaShadowPub.publish(shadowJointMsg);
/*
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_ERROR("Problema con IK");
}
ros::spinOnce();
loop_rate.sleep();
}
ros::shutdown();
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "state_display_tutorial");
/* Needed for ROS_INFO commands to work */
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();
/* 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));
/* Get the configuration for the joints in the right arm of the PR2*/
robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm");
/* PUBLISH RANDOM ARM POSITIONS */
ros::NodeHandle nh;
ros::Publisher robot_state_publisher = nh.advertise<moveit_msgs::DisplayRobotState>( "tutorial_robot_state", 1 );
/* loop at 1 Hz */
ros::Rate loop_rate(1);
for (int cnt=0; cnt<5 && ros::ok(); cnt++)
{
joint_state_group->setToRandomValues();
/* get a robot state message describing the pose in kinematic_state */
moveit_msgs::DisplayRobotState msg;
robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state);
/* send the message to the RobotState display */
robot_state_publisher.publish( msg );
/* let ROS send the message, then wait a while */
ros::spinOnce();
loop_rate.sleep();
}
/* POSITION END EFFECTOR AT SPECIFIC LOCATIONS */
/* Find the default pose for the end effector */
kinematic_state->setToDefaultValues();
const Eigen::Affine3d end_effector_default_pose =
kinematic_state->getLinkState("r_wrist_roll_link")->getGlobalLinkTransform();
const double PI = boost::math::constants::pi<double>();
const double RADIUS = 0.1;
for (double angle=0; angle<=2*PI && ros::ok(); angle+=2*PI/20)
{
/* calculate a position for the end effector */
Eigen::Affine3d end_effector_pose =
Eigen::Translation3d(RADIUS * cos(angle), RADIUS * sin(angle), 0.0) * end_effector_default_pose;
ROS_INFO_STREAM("End effector position:\n" << end_effector_pose.translation());
/* use IK to get joint angles satisfyuing the calculated position */
bool found_ik = joint_state_group->setFromIK(end_effector_pose, 10, 0.1);
if (!found_ik)
{
ROS_INFO_STREAM("Could not solve IK for pose\n" << end_effector_pose.translation());
continue;
}
/* get a robot state message describing the pose in kinematic_state */
moveit_msgs::DisplayRobotState msg;
robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state);
/* send the message to the RobotState display */
robot_state_publisher.publish( msg );
/* let ROS send the message, then wait a while */
ros::spinOnce();
loop_rate.sleep();
}
ros::shutdown();
return 0;
}