Cartesian(XYZ空間）とジョイント空間を行き来する。
http://www.ros.org/wiki/pr2/Tutorials/Finding%20Cartesian%20poses%20from%20joint%20angles%20and%20vice%20versa
チュートリアルではIKとは何かということを簡単に説明しています。
分かる人には分かるけど、分からない人にはなかなか分からないので割愛します。
とにかく、アームは７DOFだけど、ちゃんと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は最初か３つ目の関節角度が０になるように計算するみたいです。
順運動学も入力と出力が逆ですがほぼ同じですね。
現状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 件のコメント:
コメントを投稿