Determining Joint Rotations in Gazebo using KDL

Question

I am trying to use the Kinematic Dynamics Library (KDL) within Gazebo to make a "robot arm" position it's palm onto a target model. My current issue is that when I rotate my joints to the computed values, my robot is not at all where I expect it to be and does not touch the target cylinder. I have tried asking the Gazebo community for an answer, but I have not gotten a response so I was hoping that someone at stack overflow could be of assistance. Pasted below is the relevant code:

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