Computing Joint Rotations Using KDL [closed]

asked 2013-02-24 17:47:03 -0500

chrismflynn gravatar image

I am trying to use the Kinematic Dynamics Library (KDL) within Gazebo to make a "robot arm" position it's palm onto a cylinder. 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. Pasted below is the relevant code from my plugin

//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 ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by nkoenig
close date 2013-09-19 10:03:29.291343

Comments

Is the robot model in the wrong position, or do the joints end up in the wrong configuration?

nkoenig gravatar imagenkoenig ( 2013-03-07 22:32:55 -0500 )edit

The joints end up in the wrong configuration. I ended up doing a custom solution since I could reduce my joint rotation to a 2 dimensional problem.

chrismflynn gravatar imagechrismflynn ( 2013-03-13 11:35:36 -0500 )edit