2010年1月13日水曜日

PR2のアームを逆運動学を使って動かす

今回はPR2のアームを、前回やった逆運動学を使って動かします。

http://www.ros.org/wiki/pr2/Tutorials/Moving%20the%20arm%20through%20a%20Cartesian%20pose%20trajectory

じゃあ、まず
$ roslaunch pr2_gazebo pr2_empty_world.launch
して、

$ rosrun pr2_controller_manager pr2_controller_manager list
で、コントローラーが動いているのを確かめましょう。

r_arm_controller (running)
ならOKです。

今回作るサンプルは、XYZ空間の座標列を貰い、それを手先で追従するようなサービスを作ります。

ではまずパッケージ作成。
$ roscreate-pkg ik_trajectory_tutorial actionlib roscpp joint_trajectory_action geometry_msgs manipulation_srvs pr2_arm_ik kinematics_msgs joint_states_listener pr2_controllers_msgs tf
うわー、めっちゃ依存多いですね。まあ、大体みたことあるものですね。
manipulation_srvsだけないかな?

まずはサービスの型(メッセージ)を決めます。
ヘッダと空間座標のposesを受け取り、成否を返すメッセージです。
以下をsrv/ExecuteCartesianIKTrajectory.srvとして保存しましょう。
Header header
geometry_msgs/Pose[] poses
---
uint32 success

CMakeLists.txtのrosbuild_gensrvをコメントインして、
rosmakeしておきましょう。

では以下をsrc/ik_trajectory_tutorial.cppとして保存です。

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <ros/ros.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include <kinematics_msgs/IKQuery.h>
#include <kinematics_msgs/IKService.h>
#include <kinematics_msgs/FKService.h>
#include <joint_states_listener/ReturnJointStates.h>
#include <ik_trajectory_tutorial/ExecuteCartesianIKTrajectory.h>
#include <vector>
#define MAX_JOINT_VEL 0.5 //in radians/sec static const std::string ARM_IK_NAME = "/pr2_ik_right_arm/ik_service"; typedef actionlib::SimpleActionClient<pr2_controllers_msgs:: JointTrajectoryAction> TrajClient; class IKTrajectoryExecutor{ private: ros::NodeHandle node; ros::ServiceClient ik_client; ros::ServiceClient joint_states_client; ros::ServiceServer service; pr2_controllers_msgs::JointTrajectoryGoal goal; kinematics_msgs::IKService::Request ik_request; kinematics_msgs::IKService::Response ik_response; TrajClient *action_client; public: IKTrajectoryExecutor(){ //create a client function for the IK service ik_client = node.serviceClient<kinematics_msgs::IKService> (ARM_IK_NAME, true); //tell the joint trajectory action client that we want //to spin a thread by default action_client = new TrajClient("r_arm_controller/joint_trajectory_action", true); //wait for the action server to come up while(!action_client->waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the joint_trajectory_action action server to come up"); } //use the return_joint_states service to get the current joint angles joint_states_client = node.serviceClient <joint_states_listener::ReturnJointStates>("return_joint_states"); //wait for the return_joint_states service to be ready ROS_INFO("Waiting for return_joint_states service to be ready\n"); ros::service::waitForService("return_joint_states"); //register a service to input desired Cartesian trajectories service = node.advertiseService("execute_cartesian_ik_trajectory", &IKTrajectoryExecutor::execute_cartesian_ik_trajectory, this); //have to specify the order of the joints we're sending in our //joint trajectory goal, even if they're already on the param server goal.trajectory.joint_names.push_back("r_shoulder_pan_joint"); goal.trajectory.joint_names.push_back("r_shoulder_lift_joint"); goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint"); goal.trajectory.joint_names.push_back("r_elbow_flex_joint"); goal.trajectory.joint_names.push_back("r_forearm_roll_joint"); goal.trajectory.joint_names.push_back("r_wrist_flex_joint"); goal.trajectory.joint_names.push_back("r_wrist_roll_joint"); } ~IKTrajectoryExecutor(){ delete action_client; } //run inverse kinematics on a PoseStamped (7-dof pose //(position + quaternion orientation) + header specifying the //frame of the pose) //tries to stay close to double positions[7] //returns the solution angles in double solution[7] bool run_ik(geometry_msgs::PoseStamped pose, double positions[7], double solution[7]){ kinematics_msgs::IKService::Request ik_request; kinematics_msgs::IKService::Response ik_response; ik_request.data.joint_names.push_back("r_shoulder_pan_joint"); ik_request.data.joint_names.push_back("r_shoulder_lift_joint"); ik_request.data.joint_names.push_back("r_upper_arm_roll_joint"); ik_request.data.joint_names.push_back("r_elbow_flex_joint"); ik_request.data.joint_names.push_back("r_forearm_roll_joint"); ik_request.data.joint_names.push_back("r_wrist_flex_joint"); ik_request.data.joint_names.push_back("r_wrist_roll_joint"); ik_request.data.link_name = "r_wrist_roll_link"; ik_request.data.pose_stamped = pose; ik_request.data.positions.resize(7); for(int i=0; i<7; i++) ik_request.data.positions[i] = positions[i]; bool ik_service_call = ik_client.call(ik_request,ik_response); if(!ik_service_call){ ROS_ERROR("IK service call failed!"); return 0; } for(int i=0; i<7; i++){ solution[i] = ik_response.solution[i]; } ROS_INFO("solution angles: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", solution[0],solution[1], solution[2],solution[3], solution[4],solution[5],solution[6]); ROS_INFO("IK service call succeeded"); return 1; } //figure out where the arm is now bool get_current_joint_angles(double current_angles[7]){ int i; joint_states_listener::ReturnJointStates joint_states_srv; joint_states_srv.request.name.push_back("r_shoulder_pan_joint"); joint_states_srv.request.name.push_back("r_shoulder_lift_joint"); joint_states_srv.request.name.push_back("r_upper_arm_roll_joint"); joint_states_srv.request.name.push_back("r_elbow_flex_joint"); joint_states_srv.request.name.push_back("r_forearm_roll_joint"); joint_states_srv.request.name.push_back("r_wrist_flex_joint"); joint_states_srv.request.name.push_back("r_wrist_roll_joint"); if(joint_states_client.call(joint_states_srv)){ for(i=0; i<7; i++) current_angles[i] = joint_states_srv.response.position[i]; return 1; } else{ ROS_ERROR("Failed to call service joint_states_srv"); return 0; } } //send a desired joint trajectory to the joint trajectory executor //and wait for it to finish bool execute_joint_trajectory(std::vector<double *> joint_trajectory){ int i, j; int trajectorylength = joint_trajectory.size(); //get the current joint angles double current_angles[7]; get_current_joint_angles(current_angles); //fill the goal message with the desired joint trajectory goal.trajectory.points.resize(trajectorylength+1); //set the first trajectory point to the current position goal.trajectory.points[0].positions.resize(7); goal.trajectory.points[0].velocities.resize(7); for(j=0; j<7; j++){ goal.trajectory.points[0].positions[j] = current_angles[j]; goal.trajectory.points[0].velocities[j] = 0.0; } //make the first trajectory point start 0.5 seconds from when we run goal.trajectory.points[0].time_from_start = ros::Duration(0.5); //fill in the rest of the trajectory double time_from_start = 0.; for(i=0; i<trajectorylength; i++){ printf("goal point %d: ", i); goal.trajectory.points[i+1].positions.resize(7); goal.trajectory.points[i+1].velocities.resize(7); //fill in the joint positions (velocities of 0 mean that the arm //will try to stop briefly at each waypoint) for(j=0; j<7; j++){ printf("%0.3f ", joint_trajectory[i][j]); goal.trajectory.points[i+1].positions[j] = joint_trajectory[i][j]; goal.trajectory.points[i+1].velocities[j] = 0.0; } printf("\n"); //compute a desired time for this trajectory point using a max //joint velocity double max_joint_move = 0; for(j=0; j<7; j++){ double joint_move = fabs(goal.trajectory.points[i+1].positions[j] - goal.trajectory.points[i].positions[j]); if(joint_move > max_joint_move) max_joint_move = joint_move; } double seconds = max_joint_move/MAX_JOINT_VEL; ROS_INFO("max_joint_move: %0.3f, seconds: %0.3f", max_joint_move, seconds); time_from_start += seconds; goal.trajectory.points[i+1].time_from_start = ros::Duration(time_from_start); printf("\n"); } //when to start the trajectory goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5); ROS_INFO("Sending goal to joint_trajectory_action"); action_client->sendGoal(goal); action_client->waitForResult(); //get the current joint angles for debugging get_current_joint_angles(current_angles); ROS_INFO("joint angles after trajectory: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f\n", current_angles[0],current_angles[1], current_angles[2],current_angles[3],current_angles[4],current_angles[5],current_angles[6]); if(action_client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){ ROS_INFO("Hooray, the arm finished the trajectory!"); return 1; } ROS_INFO("The arm failed to execute the trajectory."); return 0; } //service function for execute_cartesian_ik_trajectory bool execute_cartesian_ik_trajectory( ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Request &req, ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Response &res){ int trajectory_length = req.poses.size(); int i, j; //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[7]; 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[7]; success = run_ik(stamped_pose, last_angles, trajectory_point); 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<7; 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){ //init ROS node ros::init(argc, argv, "cartesian_ik_trajectory_executor"); //sleep(1); IKTrajectoryExecutor ik_traj_exec = IKTrajectoryExecutor(); ROS_INFO("Waiting for cartesian trajectories to execute"); ros::spin(); return 0; }


かなり長いです。
でも大体がこれまでやったことと同じですので分かると思います。

実行するためのクライアントが以下です。
#test client for ik_trajectory_tutorial

import roslib
roslib.load_manifest('ik_trajectory_tutorial')
import rospy
from ik_trajectory_tutorial.srv import ExecuteCartesianIKTrajectory
from geometry_msgs.msg import Pose
import time
import sys
import pdb
import tf

#execute a Cartesian trajectory defined by a root frame, a list of 
#positions (x,y,z), and a list of orientations (quaternions: x,y,z,w)
def call_execute_cartesian_ik_trajectory(frame, positions, orientations):
    rospy.wait_for_service("execute_cartesian_ik_trajectory")

    #fill in the header (don't need seq)
    header = roslib.msg.Header()
    header.frame_id = frame
    header.stamp = rospy.get_rostime()

    #fill in the poses
    poses = []
    for (position, orientation) in zip(positions, orientations):
        pose = Pose()
        pose.position.x = position[0]
        pose.position.y = position[1]
        pose.position.z = position[2]
        pose.orientation.x = orientation[0]
        pose.orientation.y = orientation[1]
        pose.orientation.z = orientation[2]
        pose.orientation.w = orientation[3]
        poses.append(pose)

    #call the service to execute the trajectory
    print "calling execute_cartesian_ik_trajectory"
    try:
        s = rospy.ServiceProxy("execute_cartesian_ik_trajectory", \
                                   ExecuteCartesianIKTrajectory)
        resp = s(header, poses)
    except rospy.ServiceException, e:
        print "error when calling execute_cartesian_ik_trajectory: %s"%e
        return 0
    return resp.success

#pretty-print list to string
def pplist(list):
    return ' '.join(['%2.3f'%x for x in list])

#print out the positions, velocities, and efforts of the right arm joints
if __name__ == "__main__":
    rospy.init_node("test_cartesian_ik_trajectory_executer")
    tf_listener = tf.TransformListener()
    time.sleep(.5) #give the transform listener time to get some frames

    joint_names = ["r_shoulder_pan_joint",
                   "r_shoulder_lift_joint",
                   "r_upper_arm_roll_joint",
                   "r_elbow_flex_joint",
                   "r_forearm_roll_joint",
                   "r_wrist_flex_joint",
                   "r_wrist_roll_joint"]

    positions = [[.76, -.19, .83], [0.59, -0.36, 0.93]]
    orientations = [[.02, -.09, 0.0, 1.0], [0.65, -0.21, .38, .62]]

    success = call_execute_cartesian_ik_trajectory("base_link", \
            positions, orientations)

    #check the final pose
    (trans, rot) = tf_listener.lookupTransform('base_link', 'r_wrist_roll_link', rospy.Time(0))
    print "end Cartesian pose: trans", pplist(trans), "rot", pplist(rot)

    if success:
        print "trajectory succeeded!"
    else:
        print "trajectory failed."
なぜかjoint_namesいれてるけど、意味ないですね。


動かしてみると、こんな感じ。


グラフィカルに手先をぐいぐい引っ張るインタフェース欲しいですね。
今回はサンプル長いのでいったん切ります。

0 件のコメント:

コメントを投稿