int main(int argc, char *argv[]) { ros::init (argc, argv, "kinematic_model_tutorial"); ros::NodeHandle nh; 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 */ 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()); /* Create a kinematic state - this represents the configuration for * the robot represented by kinematic_model */ kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model)); /* Set all joints in this state to their default values */ kinematic_state->setToDefaultValues(); joint_model_group = kinematic_model->getJointModelGroup("arm_1"); moveitIKPseudoInverseSolver(); ROS_INFO_STREAM("Test complete."); return -1; }
void renderHierarchicalTrajectory( robot_trajectory::RobotTrajectoryPtr& robot_trajectory, ros::NodeHandle& node_handle, robot_model::RobotModelPtr& robot_model) { static ros::Publisher vis_marker_array_publisher_ = node_handle.advertise< visualization_msgs::MarkerArray>("itomp_planner/trajectory", 10); visualization_msgs::MarkerArray ma; std::vector<std::string> link_names = robot_model->getLinkModelNames(); std_msgs::ColorRGBA color; std::string ns = "robot"; ros::Duration dur(100.0); std::map<std::string, std_msgs::ColorRGBA> colorMap; color.a = 0.3; color.r = 0.5; color.g = 1.0; color.b = 1.0; colorMap["lower_body"] = color; color.a = 0.3; color.r = 0.5; color.g = 1.0; color.b = 0.3; colorMap["torso"] = color; color.g = 0.3; color.b = 1.0; colorMap["head"] = color; color.r = 0.9; color.g = 0.5; color.b = 0.3; colorMap["left_arm"] = color; color.r = 0.9; color.g = 0.3; color.b = 0.5; colorMap["right_arm"] = color; color.r = 1.0; color.g = 1.0; color.b = 1.0; colorMap["object"] = color; const robot_state::JointModelGroup* joint_model_group; std::map<std::string, std::vector<std::string> > group_links_map; group_links_map["lower_body"] = robot_model->getJointModelGroup( "lower_body")->getLinkModelNames(); group_links_map["torso"] = robot_model->getJointModelGroup("torso")->getLinkModelNames(); group_links_map["head"] = robot_model->getJointModelGroup("head")->getLinkModelNames(); group_links_map["left_arm"] = robot_model->getJointModelGroup("left_arm")->getLinkModelNames(); group_links_map["right_arm"] = robot_model->getJointModelGroup("right_arm")->getLinkModelNames(); group_links_map["object"].clear(); if (robot_model->hasLinkModel("right_hand_object_link")) group_links_map["object"].push_back("right_hand_object_link"); int num_waypoints = robot_trajectory->getWayPointCount(); for (int i = 0; i < num_waypoints; ++i) { ma.markers.clear(); robot_state::RobotStatePtr state = robot_trajectory->getWayPointPtr(i); for (std::map<std::string, std::vector<std::string> >::iterator it = group_links_map.begin(); it != group_links_map.end(); ++it) { std::string ns = "robot_" + it->first; state->getRobotMarkers(ma, group_links_map[it->first], colorMap[it->first], ns, dur); } vis_marker_array_publisher_.publish(ma); double time = (i == 0 || i == num_waypoints - 1) ? 2.0 : 0.05; ros::WallDuration timer(time); timer.sleep(); } for (int i = 0; i < 10; ++i) { ma.markers.clear(); robot_state::RobotStatePtr state = robot_trajectory->getWayPointPtr( num_waypoints - 1); for (std::map<std::string, std::vector<std::string> >::iterator it = group_links_map.begin(); it != group_links_map.end(); ++it) { std::string ns = "robot_" + it->first; state->getRobotMarkers(ma, group_links_map[it->first], colorMap[it->first], ns, dur); } vis_marker_array_publisher_.publish(ma); double time = 0.05; ros::WallDuration timer(time); timer.sleep(); } }