ROS相關筆記

獲得當前機械臂六軸弧度，並修改：

std::vector<double> group_variable_values;
group->getCurrentState()->copyJointGroupPositions(group->getCurrentState()->getRobotModel()->getJointModelGroup(group->getName()), group_variable_values);

group_variable_values[5] = 3.14/180*(0-group_variable_values[5]/3.14*180);
group->setJointValueTarget(group_variable_values);
success = group->plan(plan);//第二次規劃

添加障礙物，要ASCii格式的STL文件：

         moveit_msgs::CollisionObject toilet;
         shapes::Mesh* m = shapes::createMeshFromResource("package://rospc/src/chilun.STL");
         shape_msgs::Mesh toilet_mesh;
         shapes::ShapeMsg toilet_mesh_msg;
         shapes::constructMsgFromShape(m,toilet_mesh_msg);
         toilet_mesh = boost::get<shape_msgs::Mesh>(toilet_mesh_msg);
         toilet.meshes.resize(1);
         toilet.id = "chilun.stl";
         toilet.meshes[0] = toilet_mesh;
         toilet.mesh_poses.resize(1);
         toilet.mesh_poses[0].position.x = 0;
         toilet.mesh_poses[0].position.y = 0;
         toilet.mesh_poses[0].position.z = 0;
         toilet.mesh_poses[0].orientation.w= 1;
         toilet.mesh_poses[0].orientation.x= 0;
         toilet.mesh_poses[0].orientation.y= 0;
         toilet.mesh_poses[0].orientation.z= 0;
         //pub_co.publish(toilet);

         toilet.meshes.push_back(toilet_mesh);
         toilet.mesh_poses.push_back(toilet.mesh_poses[0]);
         toilet.operation = toilet.ADD;

         collision_objects.push_back(toilet);
         ROS_INFO("Add obstacles.");
         current_scene.addCollisionObjects(collision_objects);

添加關節約束：

         group->allowReplanning(true);
         moveit_msgs::JointConstraint jcm;
         jcm.joint_name = "joint_1";
         jcm.position = 0;
         jcm.tolerance_above = 3.14;
         jcm.tolerance_below = 0;
         jcm.weight = 1.0;

         // Applying the joint constraints
         moveit_msgs::Constraints test_constraints;
         test_constraints.joint_constraints.push_back(jcm); 
         group->setPathConstraints(test_constraints);

四元數 弧度 旋轉矩陣之間的轉換：

#include <iostream>
#include <Eigen/Eigen>
#include <stdlib.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <vector>
#include <math.h>
 
using namespace std;
using namespace Eigen;
 
Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitX());

    //Eigen::Affine3f rz= 
    
    //Eigen::Affine3f r = rollAngle * pitchAngle * yawAngle;
    //Eigen::Affine3f t (Eigen::Translation3f(Eigen::Vector3f(-0.0180369,-0.00529433,0.0589791)));
    //Eigen::Matrix4f tl1 = ( t * r).matrix();
    //cout << "tf1=" << endl << tf1.x() <<endl;
 
    Eigen::Quaterniond q = rollAngle *  pitchAngle * yawAngle;
    cout << "Euler2Quaternion result is:" <<endl;
    cout << "x = " << q.x() <<endl;
    cout << "y = " << q.y() <<endl;
    cout << "z = " << q.z() <<endl;
    cout << "w = " << q.w() <<endl<<endl;
    return q;
}
 
Eigen::Vector3d Quaterniond2Euler(const double x,const double y,const double z,const double w)
{
    Eigen::Quaterniond q;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    q.w() = w;
 
    Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
    cout << "Quaterniond2Euler result is:" <<endl;
    cout << "x = "<< euler[2] << endl ;
    cout << "y = "<< euler[1] << endl ;
    cout << "z = "<< euler[0] << endl << endl;
}
 
Eigen::Matrix4d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)
{
    Eigen::Quaterniond q;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    q.w() = w;
    
    //Eigen::Matrix3d R = q.normalized().toRotationMatrix();
    Eigen::Matrix3d R = q.toRotationMatrix();
    Eigen::Vector3d T(-0.0180369,-0.00529433,0.0589791);
    Eigen::Matrix4d Trans;
    Trans.setIdentity();
    Trans.block<3,3>(0,0)= R;
    Trans.block<3,1>(0,3)= T;
    cout << "Quaternion2RotationMatrix result is:" <<endl;
    cout << "R = " << endl << R << endl<< endl;
    return Trans;
}
 
 
Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)
{
    Eigen::Quaterniond q = Eigen::Quaterniond(R);
    q.normalize();
    cout << "RotationMatrix2Quaterniond result is:" <<endl;
    cout << "x = " << q.x() <<endl;
    cout << "y = " << q.y() <<endl;
    cout << "z = " << q.z() <<endl;
    cout << "w = " << q.w() <<endl<<endl;
    return q;
}
 
Eigen::Matrix3d euler2RotationMatrix(const double roll, const double pitch, const double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitX());
 
    Eigen::Quaterniond q = rollAngle * pitchAngle * yawAngle;
    Eigen::Matrix3d R = q.matrix();
    cout << "Euler2RotationMatrix result is:" <<endl;
    cout << "R = " << endl << R << endl<<endl;
    return R;
}
 
Eigen::Vector3d RotationMatrix2euler(Eigen::Matrix3d R)
{
    Eigen::Matrix3d m;
    m = R;
    Eigen::Vector3d euler = m.eulerAngles(0, 1, 2);
    cout << "RotationMatrix2euler result is:" << endl;
    cout << "x = "<< euler[2] << endl ;
    cout << "y = "<< euler[1] << endl ;
    cout << "z = "<< euler[0] << endl << endl;
    return euler;
}
 int main(int argc, char **argv)
{
 
//this is euler2Quaternion transform function,please input your euler angle//
  euler2Quaternion(260.143/57.29578,7.29605/57.29578,236.717/57.29578);
 
//this is Quaternion2Euler transform function,please input your euler angle//
  Quaterniond2Euler(0.847359,-0.49922, 0.141465, 0.112913);
 
//this is Quaternion2RotationMatrix transform function,please input your Quaternion parameter//
   Eigen::Matrix4d Trans1 =Quaternion2RotationMatrix( 1,0,0,0);
 
//this is rotationMatrix2Euler transform function,please input your RotationMatrix parameter like following//
  Eigen::Vector3d x_axiz,y_axiz,z_axiz;

  x_axiz << 0.461532, -0.81409 ,  0.35248;
  y_axiz << -0.877983,-0.476058, 0.0501112;
  z_axiz << 0.127006, -0.3326, -0.934477;
  Eigen::Matrix3d R;
  R << x_axiz,y_axiz,z_axiz;
  rotationMatrix2Quaterniond(R);
 
//this is euler2RotationMatrix transform function,please input your euler angle for the function parameter//
  Eigen::Matrix3d R1= euler2RotationMatrix(88.24/57.29578,358.22/57.29578,160.85/57.29578);
  Eigen::Vector3d T1(0.38901,-0.14556,0.43634);
    Eigen::Matrix4d Trans;
    Trans.setIdentity();
    Trans.block<3,3>(0,0)= R1;
    Trans.block<3,1>(0,3)= T1;
  Eigen::Matrix4d Trans2=Trans* Trans1;
  cout << "SS = " << endl << Trans2 << endl<< endl;

  Eigen::Matrix4d toolinbase;
  toolinbase<<  1,0,0,0.072538,
                0, 1,0,-0.115280,
                0,0,1 ,0.933838,
                0,0 ,0,1;
         //cout << "toolinbase = " << endl << toolinbase << endl<<endl;
         Eigen::Matrix4d camintool;
         camintool<<  -0.859032,-0.510926,0.0318573,-0.0180369,
                      0.511701, -0.858817,0.0243431,-0.00529433,
                      0.0149221,0.0372129,0.999196 ,0.0589791,
                      0        ,0        ,0        ,1;
         Eigen::Matrix4d caminbase = toolinbase*camintool;
         cout << "caminbase = " << endl << caminbase << endl<<endl;
         Eigen::Vector4d objincampose(-0.0249,-0.0202,0.2,1);
         Eigen::Vector4d objinbasepose = caminbase*objincampose;
    cout << "x = "<< objinbasepose[0] << endl ;
    cout << "y = "<< objinbasepose[1] << endl ;
    cout << "z = "<< objinbasepose[2] << endl << endl;

 
//this is RotationMatrix2euler transform function,please input your euler angle for the function parameter//
  RotationMatrix2euler(R);
 
  cout << "All transform is done!" << endl;
}