Exemplo n.º 1
0
  //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;
  }
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
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;
}
Exemplo n.º 4
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;
  
}
Exemplo n.º 5
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;
}
Exemplo n.º 7
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();
  }
}
Exemplo n.º 8
0
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();  
  }
  
}
Exemplo n.º 9
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;
}
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;
}
Exemplo n.º 11
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;
}
Exemplo n.º 12
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 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;
}
Exemplo n.º 13
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;
}