robotics - KDL を使用した Gazebo での関節回転の決定

Question

Gazebo 内でキネマティック ダイナミクス ライブラリ (KDL) を使用して、「ロボット アーム」の位置をターゲット モデルの手のひらにしようとしています。現在の問題は、ジョイントを計算された値に回転させると、ロボットが期待する場所にまったくなく、ターゲット シリンダーに触れないことです。Gazebo コミュニティに回答を求めてみましたが、回答が得られなかったので、スタック オーバーフローの誰かが助けてくれることを期待していました。以下に貼り付けたのは、関連するコードです。

  //Create a KDL Joint Chain of our Arm Robot
  KDL::Chain chain;

  //Base Joint
  math::Vector3 basePose = mBaseJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), 
  KDL::Frame(KDL::Vector(basePose.x, basePose.y, basePose.z))));

  //Shoulder Joint
  math::Vector3 shoulderPose = mShoulderJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotX), 
    KDL::Frame(KDL::Vector(shoulderPose.x, shoulderPose.y, shoulderPose.z))));

  //First Elbow Joint
  math::Vector3 firstElbowPose = mFirstElbowJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotX), 
    KDL::Frame(KDL::Vector(firstElbowPose.x, firstElbowPose.y, firstElbowPose.z))));

  //Second Elbow Joint
  math::Vector3 secondElbowPose = mSecondElbowJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotX), 
    KDL::Frame(KDL::Vector(secondElbowPose.x, secondElbowPose.y, secondElbowPose.z))));

  //End Joint
  math::Vector3 endPose = mWristJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None),
    KDL::Frame(KDL::Vector(endPose.x, endPose.y, endPose.z))));

  KDL::ChainIkSolverPos_LMA myIk = KDL::ChainIkSolverPos_LMA(chain);         

  //Create joint array
  jointCount = chain.getNrOfJoints();

  KDL::JntArray jointPositions = KDL::JntArray(jointCount);
  targetRotation = KDL::JntArray( jointCount );

  for(int i = 0; i < jointCount - 1; i++)
  {
    jointPositions(i) = mJoints[i]->GetAngle(0).Radian();
  }

  physics::ModelPtr target = mModel->GetWorld()->GetModel("Target");

  math::Pose pose = target->GetWorldPose();
  math::Pose myPose = mModel->GetWorldPose();

  double xPosition = pose.pos.x - myPose.pos.x;
  double yPosition = pose.pos.y - myPose.pos.y;
  double zPosition = pose.pos.z - myPose.pos.z;

  KDL::Frame cartesianPosition = KDL::Frame(KDL::Vector(xPosition, yPosition, zPosition));

  gzdbg << "Calculating..." << std::endl;
  bool kinematics_status;
  kinematics_status = myIk.CartToJnt( jointPositions, cartesianPosition, targetRotation );
  gzdbg << "Final Status: " << kinematics_status << std::endl;
  if(kinematics_status >= 0)
  {
    for( int i = 0; i < jointCount - 1; i++ )
    {
      mJoints[i]->SetAngle(0, targetRotation(i));
    }
  }//Create a KDL Joint Chain of our Arm Robot
  KDL::Chain chain;

  //Base Joint
  math::Vector3 basePose = mBaseJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), 
  KDL::Frame(KDL::Vector(basePose.x, basePose.y, basePose.z))));

  //Shoulder Joint
  math::Vector3 shoulderPose = mShoulderJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotX), 
    KDL::Frame(KDL::Vector(shoulderPose.x, shoulderPose.y, shoulderPose.z))));

  //First Elbow Joint
  math::Vector3 firstElbowPose = mFirstElbowJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotX), 
    KDL::Frame(KDL::Vector(firstElbowPose.x, firstElbowPose.y, firstElbowPose.z))));

  //Second Elbow Joint
  math::Vector3 secondElbowPose = mSecondElbowJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotX), 
    KDL::Frame(KDL::Vector(secondElbowPose.x, secondElbowPose.y, secondElbowPose.z))));

  //End Joint
  math::Vector3 endPose = mWristJoint->GetAnchor(0);
  chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None),
    KDL::Frame(KDL::Vector(endPose.x, endPose.y, endPose.z))));

  KDL::ChainIkSolverPos_LMA myIk = KDL::ChainIkSolverPos_LMA(chain);         

  //Create joint array
  jointCount = chain.getNrOfJoints();

  KDL::JntArray jointPositions = KDL::JntArray(jointCount);
  targetRotation = KDL::JntArray( jointCount );

  for(int i = 0; i < jointCount - 1; i++)
  {
    jointPositions(i) = mJoints[i]->GetAngle(0).Radian();
  }

  physics::ModelPtr target = mModel->GetWorld()->GetModel("Target");

  math::Pose pose = target->GetWorldPose();
  math::Pose myPose = mModel->GetWorldPose();

  double xPosition = pose.pos.x - myPose.pos.x;
  double yPosition = pose.pos.y - myPose.pos.y;
  double zPosition = pose.pos.z - myPose.pos.z;

  KDL::Frame cartesianPosition = KDL::Frame(KDL::Vector(xPosition, yPosition, zPosition));

  gzdbg << "Calculating..." << std::endl;
  bool kinematics_status;
  kinematics_status = myIk.CartToJnt( jointPositions, cartesianPosition, targetRotation );
  gzdbg << "Final Status: " << kinematics_status << std::endl;
  if(kinematics_status >= 0)
  {
    for( int i = 0; i < jointCount - 1; i++ )
    {
      mJoints[i]->SetAngle(0, targetRotation(i));
    }
  }