//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; }