2010年1月11日月曜日

逆運動学

今回は逆運動学です。

Cartesian(XYZ空間)とジョイント空間を行き来する。
http://www.ros.org/wiki/pr2/Tutorials/Finding%20Cartesian%20poses%20from%20joint%20angles%20and%20vice%20versa

チュートリアルではIKとは何かということを簡単に説明しています。
分かる人には分かるけど、分からない人にはなかなか分からないので割愛します。
とにかく、アームは7DOFだけど、ちゃんとIKとけますよ。ということです。



$ roscreate-pkg ik_tutorial roscpp geometry_msgs pr2_arm_ik kinematics_msgs
でパッケージ作成します。

frame_idとPoseをペアにして使います。Poseはx,y,zの座標とクオータニオン(姿勢)をセットにしたものです。

今回のサンプルは、ランダムに関節角度を決めて、そのときの手先位置を順運動学で計算。そして、今度は逆にその位置から逆運動学を解いて関節角度を求める。ということをやります。
逆運動学の成否は、解いた結果を順運動学して、入力の座標と比較することで確認します。

では以下をsrc/ik_tutorial.cppとして保存しましょう。
むちゃくちゃ長い!!

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <ros/ros.h>
#include <kinematics_msgs/IKQuery.h>
#include <kinematics_msgs/IKService.h>
#include <kinematics_msgs/FKService.h>
#include <vector>
#define IK_NEAR 1e-4 #define IK_NEAR_TRANSLATE 1e-5 static const std::string ARM_FK_NAME = "/pr2_ik_right_arm/fk_service"; static const std::string ARM_IK_NAME = "/pr2_ik_right_arm/ik_service"; static const std::string ARM_QUERY_NAME = "/pr2_ik_right_arm/ik_query"; double gen_rand(double min, double max) { int rand_num = rand()%100+1; double result = min + (double)((max-min)*rand_num)/101.0; return result; } bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR) { if(fabs(v1-v2) > NEAR) return true; return false; } class TestIKandFK{ private: ros::NodeHandle node; ros::ServiceClient ik_client; ros::ServiceClient fk_client; ros::ServiceClient query_client; public: TestIKandFK(){ //create a client function for the IK, FK, and joint query services ik_client = node.serviceClient<kinematics_msgs::IKService> (ARM_IK_NAME, true); fk_client = node.serviceClient<kinematics_msgs::FKService> (ARM_FK_NAME, true); query_client = node.serviceClient<kinematics_msgs::IKQuery> (ARM_QUERY_NAME, true); //wait for the various services to be ready ROS_INFO("Waiting for services to be ready"); ros::service::waitForService(ARM_IK_NAME); ros::service::waitForService(ARM_FK_NAME); ros::service::waitForService(ARM_QUERY_NAME); ROS_INFO("Services ready"); } //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 start_angles[7] //returns the solution angles in double solution[7] bool run_ik(geometry_msgs::PoseStamped pose, double start_angles[7], double solution[7], std::string link_name){ 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 = link_name; ik_request.data.pose_stamped = pose; ik_request.data.positions.resize(7); for(int i=0; i<7; i++) ik_request.data.positions[i] = start_angles[i]; ROS_INFO("request pose: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f", pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w); 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; } //run forward kinematics on a set of 7 joint angles bool run_fk(double angles[7], geometry_msgs::PoseStamped &return_pose, std::string link_name){ int i; kinematics_msgs::FKService::Request fk_request; kinematics_msgs::FKService::Response fk_response; fk_request.header.stamp = ros::Time::now(); fk_request.header.frame_id = "base_link"; fk_request.joint_names.push_back("r_shoulder_pan_joint"); fk_request.joint_names.push_back("r_shoulder_lift_joint"); fk_request.joint_names.push_back("r_upper_arm_roll_joint"); fk_request.joint_names.push_back("r_elbow_flex_joint"); fk_request.joint_names.push_back("r_forearm_roll_joint"); fk_request.joint_names.push_back("r_wrist_flex_joint"); fk_request.joint_names.push_back("r_wrist_roll_joint"); fk_request.link_name = link_name; fk_request.positions.resize(7); for(i=0; i<7; i++){ fk_request.positions[i] = angles[i]; } bool success = fk_client.call(fk_request, fk_response); if(!success){ ROS_INFO("FK call failed!"); return 0; } return_pose = fk_response.pose; return 1; } //test the ik and fk functions bool run_test(){ srand ( time(NULL) ); // initialize random seed // use IKQuery to get the min and max joint angles kinematics_msgs::IKQuery::Request request; kinematics_msgs::IKQuery::Response response; query_client.call(request, response); std::vector<double> min_limits, max_limits; for(int i=0; i<7; i++){ min_limits.push_back(response.limits[i].min_position); max_limits.push_back(response.limits[i].max_position); } //run forward kinematics on a random set of joints to get //a corresponding Cartesian pose double angles[7]; for(int i=0; i<7; i++){ angles[i] = gen_rand(std::max(min_limits[i],-M_PI), std::min(max_limits[i],M_PI)); } geometry_msgs::PoseStamped fk_response; run_fk(angles, fk_response, "r_wrist_roll_link"); //run inverse kinematics on the resulting Cartesian pose to get //another set of joint angles that makes that pose double solutionangles[7]; double startangles[7] = {0,0,0,0,0,0,0}; bool success = 1; success = run_ik(fk_response, startangles, solutionangles, "r_wrist_roll_link"); if(!success){ ROS_INFO("IK call failed!\n"); return 0; } //run forward kinematics on the resulting joint angles to see //if they match the first Cartesian pose geometry_msgs::PoseStamped fk_response2; fk_response2.header.frame_id = "base_link"; run_fk(solutionangles, fk_response2, "r_wrist_roll_link"); //check to see if the two poses are pretty close to each other success = 1; if(NOT_NEAR(fk_response.pose.position.x, fk_response2.pose.position.x,IK_NEAR_TRANSLATE) || NOT_NEAR(fk_response.pose.position.y, fk_response2.pose.position.y,IK_NEAR_TRANSLATE) || NOT_NEAR(fk_response.pose.position.z, fk_response2.pose.position.z,IK_NEAR_TRANSLATE) || NOT_NEAR(fk_response.pose.orientation.x, fk_response2.pose.orientation.x,IK_NEAR) || NOT_NEAR(fk_response.pose.orientation.y, fk_response2.pose.orientation.y,IK_NEAR) || NOT_NEAR(fk_response.pose.orientation.z, fk_response2.pose.orientation.z,IK_NEAR) || NOT_NEAR(fk_response.pose.orientation.w, fk_response2.pose.orientation.w,IK_NEAR)) { ROS_INFO("IK solution incorrect\n"); ROS_INFO("ik request: %f, %f, %f :: %f %f %f %f", fk_response.pose.position.x, fk_response.pose.position.y, fk_response.pose.position.z, fk_response.pose.orientation.x, fk_response.pose.orientation.y, fk_response.pose.orientation.z, fk_response.pose.orientation.w); ROS_INFO("fk response: %f, %f, %f :: %f %f %f %f\n", fk_response2.pose.position.x, fk_response2.pose.position.y, fk_response2.pose.position.z, fk_response2.pose.orientation.x, fk_response2.pose.orientation.y, fk_response2.pose.orientation.z, fk_response2.pose.orientation.w); success = 0; } if(success){ ROS_INFO("IK solution good\n"); } else{ ROS_INFO("IK solution bad\n"); } return success; } }; int main(int argc, char** argv){ //init ROS node ros::init(argc, argv, "test_ik_and_fk"); //make the class object TestIKandFK test_ik_and_fk = TestIKandFK(); //run the test test_ik_and_fk.run_test(); return 0; }

逆運動学(IK), 順運動学(FK)はサービスとして提供されているので、サービスクライアントを利用します。
また、query_clientを使って関節角度限界を取得しています。

また、IKのrequestには関節の名前、初期角度、目標位置、その位置に合わせるリンクの名前をパックしてcallします。
するとsolutionに関節角度が帰ってきます。

IKは最初か3つ目の関節角度が0になるように計算するみたいです。

順運動学も入力と出力が逆ですがほぼ同じですね。
現状link_nameはr_wrist_roll_jointしか受け付けないみたいです。
あるあるw

IKQueryのほうは、Limitsを返しますが、limitは最小角度と最大角度だけじゃなく、
そもそも可動角限界があるか(has_position_limits)、ぐるぐる回転可能かどうか(angle_wraparound)、というデータもあります。

あとは、最初に説明したとおりですね。

ビルドは
$ rosmake ik_turorial
でやりましょう。makeでは依存関係の解消をしてくれないので。

いまさらですが、pr2 in gazeboは
$ roslaunch pr2_gazebo pr2_empty_world.launch
すると一発で起動できます。

$ roslaunch `rospack find pr2_arm_ik`/launch/pr2_ik_rarm_node.launch
でサービスを起動して、

$ bin/ik_tutorial
すると、
[ INFO] 199.410000001: Waiting for services to be ready
[ INFO] 199.413000001: Services ready
[ INFO] 199.414000001: request pose: 0.472 -0.374 1.061 0.562 -0.493 0.632 0.206
[ INFO] 199.414000001: solution angles: -0.342 -0.008 0.000 -1.393 0.858 -0.514 1.599
[ INFO] 199.414000001: IK service call succeeded
[ INFO] 199.414000001: IK solution good
という感じになります。

今回は以上です。

0 件のコメント:

コメントを投稿