用KDL确定凉亭中的联合旋转

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));
    }
  }