Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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();
	}

}