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;
}
示例#2
0
void displayStates(robot_state::RobotState& start_state,
		robot_state::RobotState& goal_state, ros::NodeHandle& node_handle,
		robot_model::RobotModelPtr& robot_model)
{
	static ros::Publisher start_state_display_publisher = node_handle.advertise<
			moveit_msgs::DisplayRobotState>("/move_itomp/display_start_state",
			1, true);
	static ros::Publisher goal_state_display_publisher = node_handle.advertise<
			moveit_msgs::DisplayRobotState>("/move_itomp/display_goal_state", 1,
			true);

	int num_variables = start_state.getVariableNames().size();

	moveit_msgs::DisplayRobotState disp_start_state;
	disp_start_state.state.joint_state.header.frame_id =
			robot_model->getModelFrame();
	disp_start_state.state.joint_state.name = start_state.getVariableNames();
	disp_start_state.state.joint_state.position.resize(num_variables);
	memcpy(&disp_start_state.state.joint_state.position[0],
			start_state.getVariablePositions(), sizeof(double) * num_variables);
	disp_start_state.highlight_links.clear();
	const std::vector<std::string>& link_model_names =
			robot_model->getLinkModelNames();
	for (int i = 0; i < link_model_names.size(); ++i)
	{
		std_msgs::ColorRGBA color;
		color.a = 0.5;
		color.r = 0.0;
		color.g = 1.0;
		color.b = 0.5;
		moveit_msgs::ObjectColor obj_color;
		obj_color.id = link_model_names[i];
		obj_color.color = color;
		disp_start_state.highlight_links.push_back(obj_color);
	}
	start_state_display_publisher.publish(disp_start_state);

	moveit_msgs::DisplayRobotState disp_goal_state;
	disp_goal_state.state.joint_state.header.frame_id =
			robot_model->getModelFrame();
	disp_goal_state.state.joint_state.name = goal_state.getVariableNames();
	disp_goal_state.state.joint_state.position.resize(num_variables);
	memcpy(&disp_goal_state.state.joint_state.position[0],
			goal_state.getVariablePositions(), sizeof(double) * num_variables);
	disp_goal_state.highlight_links.clear();
	for (int i = 0; i < link_model_names.size(); ++i)
	{
		std_msgs::ColorRGBA color;
		color.a = 0.5;
		color.r = 0.0;
		color.g = 0.5;
		color.b = 1.0;
		moveit_msgs::ObjectColor obj_color;
		obj_color.id = link_model_names[i];
		obj_color.color = color;
		disp_goal_state.highlight_links.push_back(obj_color);
	}
	goal_state_display_publisher.publish(disp_goal_state);
}
示例#3
0
void renderEnvironment(const std::string& environment_file,
		robot_model::RobotModelPtr& robot_model, const std::string& ns,
		std_msgs::ColorRGBA& color)
{
	return;

	ros::NodeHandle node_handle;
	ros::Publisher vis_marker_array_publisher_ = node_handle.advertise<
			visualization_msgs::MarkerArray>(
			"move_itomp/visualization_marker_array", 10);
	visualization_msgs::MarkerArray ma;
	visualization_msgs::Marker msg;
	msg.header.frame_id = robot_model->getModelFrame();
	msg.header.stamp = ros::Time::now();
	msg.ns = ns;
	msg.type = visualization_msgs::Marker::MESH_RESOURCE;
	msg.action = visualization_msgs::Marker::ADD;
	msg.scale.x = 1.0;
	msg.scale.y = 1.0;
	msg.scale.z = 1.0;
	msg.id = 0;
	msg.pose.position.x = 0.0;
	msg.pose.position.y = 0.0;
	msg.pose.position.z = 0.0;
	msg.pose.orientation.x = 0.0;
	msg.pose.orientation.y = 0.0;
	msg.pose.orientation.z = 0.0;
	msg.pose.orientation.w = 1.0;
	msg.color = color;
	/*
	 msg.color.a = 1.0;
	 msg.color.r = 0.5;
	 msg.color.g = 0.5;
	 msg.color.b = 0.5;
	 */
	msg.mesh_resource = environment_file;
	ma.markers.push_back(msg);
	vis_marker_array_publisher_.publish(ma);
}
示例#4
0
void displayInitialWaypoints(robot_state::RobotState& state,
                             ros::NodeHandle& node_handle,
                             robot_model::RobotModelPtr& robot_model,
                             const std::vector<std::string>& hierarchy,
                             const std::vector<Eigen::VectorXd>& waypoints)
{
    static ros::Publisher vis_marker_array_publisher = node_handle.advertise<visualization_msgs::MarkerArray>("/move_itomp/visualization_marker_array", 10);

    visualization_msgs::MarkerArray ma;
    std::vector<std::string> link_names = robot_model->getLinkModelNames();
    std_msgs::ColorRGBA color;
    color.a = 0.5;
    color.r = 1.0;
    color.g = 1.0;
    color.b = 0.0;

    //ros::Duration dur(3600.0);
    ros::Duration dur(0.25);

    for (unsigned int point = 0; point < waypoints.size(); ++point)
    {
        ma.markers.clear();

        setRobotStateFrom(state, hierarchy, waypoints, point);


        double time = 0.05;
        ros::WallDuration timer(time);
        timer.sleep();


        std::string ns = "init_" + boost::lexical_cast<std::string>(point);
        state.getRobotMarkers(ma, link_names, color, ns, dur);
        vis_marker_array_publisher.publish(ma);
    }
}
示例#5
0
void renderStaticScene(ros::NodeHandle& node_handle,
		planning_scene::PlanningScenePtr& planning_scene,
		robot_model::RobotModelPtr& robot_model)
{
	std::string environment_file;
	std::vector<double> environment_position;

	static ros::Publisher vis_marker_array_publisher_ = node_handle.advertise<
			visualization_msgs::MarkerArray>("visualization_marker_array", 10);

	ros::WallDuration(1.0).sleep();

	node_handle.param<std::string>("/itomp_planner/environment_model",
			environment_file, "");

	if (!environment_file.empty())
	{
		environment_position.resize(3, 0);
		if (node_handle.hasParam("/itomp_planner/environment_model_position"))
		{
			XmlRpc::XmlRpcValue segment;
			node_handle.getParam("/itomp_planner/environment_model_position",
					segment);
			if (segment.getType() == XmlRpc::XmlRpcValue::TypeArray)
			{
				int size = segment.size();
				for (int i = 0; i < size; ++i)
				{
					double value = segment[i];
					environment_position[i] = value;
				}
			}
		}

		visualization_msgs::MarkerArray ma;
		visualization_msgs::Marker msg;
		msg.header.frame_id = robot_model->getModelFrame();
		msg.header.stamp = ros::Time::now();
		msg.ns = "environment";
		msg.type = visualization_msgs::Marker::MESH_RESOURCE;
		msg.action = visualization_msgs::Marker::ADD;
		msg.scale.x = 1.0;
		msg.scale.y = 1.0;
		msg.scale.z = 1.0;
		msg.id = 0;
		msg.pose.position.x = environment_position[0];
		msg.pose.position.y = environment_position[1];
		msg.pose.position.z = environment_position[2];
		ROS_INFO(
				"Env render pos : (%f %f %f)", environment_position[0], environment_position[1], environment_position[2]);
		msg.pose.orientation.x = 0.0;
		msg.pose.orientation.y = 0.0;
		msg.pose.orientation.z = 0.0;
		msg.pose.orientation.w = 1.0;
		msg.color.a = 1.0;
		msg.color.r = 0.5;
		msg.color.g = 0.5;
		msg.color.b = 0.5;
		msg.mesh_resource = environment_file;
		ma.markers.push_back(msg);

		msg.ns = "environment2";
		msg.type = visualization_msgs::Marker::CUBE;
		msg.id = 0;
		msg.pose.position.x = 0;
		msg.pose.position.y = 6.0;
		msg.pose.position.z = -0.45;
		msg.scale.x = 2.0;
		msg.scale.y = 1.0;
		msg.scale.z = 1.0;
		ma.markers.push_back(msg);

		ros::WallDuration(1.0).sleep();
		vis_marker_array_publisher_.publish(ma);
	}
}
示例#6
0
void loadStaticScene(ros::NodeHandle& node_handle,
		planning_scene::PlanningScenePtr& planning_scene,
		robot_model::RobotModelPtr& robot_model)
{
	std::string environment_file;
	std::vector<double> environment_position;

	node_handle.param<std::string>("/itomp_planner/environment_model",
			environment_file, "");

	if (!environment_file.empty())
	{
		environment_position.resize(3, 0);
		if (node_handle.hasParam("/itomp_planner/environment_model_position"))
		{
			XmlRpc::XmlRpcValue segment;
			node_handle.getParam("/itomp_planner/environment_model_position",
					segment);
			if (segment.getType() == XmlRpc::XmlRpcValue::TypeArray)
			{
				int size = segment.size();
				for (int i = 0; i < size; ++i)
				{
					double value = segment[i];
					environment_position[i] = value;
				}
			}
		}

		// Collision object
		moveit_msgs::CollisionObject collision_object;
		collision_object.header.frame_id = robot_model->getModelFrame();
		collision_object.id = "environment";
		geometry_msgs::Pose pose;
		pose.position.x = environment_position[0];
		pose.position.y = environment_position[1];
		pose.position.z = environment_position[2];
		ROS_INFO(
				"Env col pos : (%f %f %f)", environment_position[0], environment_position[1], environment_position[2]);
		pose.orientation.x = 0.0;
		pose.orientation.y = 0.0;
		pose.orientation.z = 0.0;
		pose.orientation.w = 1.0;

		shapes::Mesh* shape = shapes::createMeshFromResource(environment_file);
		shapes::ShapeMsg mesh_msg;
		shapes::constructMsgFromShape(shape, mesh_msg);
		shape_msgs::Mesh mesh = boost::get<shape_msgs::Mesh>(mesh_msg);

		collision_object.meshes.push_back(mesh);
		collision_object.mesh_poses.push_back(pose);

		shape_msgs::SolidPrimitive primitive;
		primitive.type = primitive.BOX;
		primitive.dimensions.resize(3);
		primitive.dimensions[0] = 2.0;
		primitive.dimensions[1] = 1.0;
		primitive.dimensions[2] = 1.0;
		pose.position.x = 0;
		pose.position.y = 6.0;
		pose.position.z = -0.45;
		collision_object.primitive_poses.push_back(pose);
		collision_object.primitives.push_back(primitive);

		collision_object.operation = collision_object.ADD;
		moveit_msgs::PlanningScene planning_scene_msg;
		planning_scene_msg.world.collision_objects.push_back(collision_object);
		planning_scene_msg.is_diff = true;
		planning_scene->setPlanningSceneDiffMsg(planning_scene_msg);
	}
}
示例#7
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();
	}

}