Computing Joint Rotations Using KDL [closed]

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);
KDL::Frame(KDL::Vector(basePose.x, basePose.y, basePose.z))));

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

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

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

//End Joint
math::Vector3 endPose = mWristJoint->GetAnchor(0);
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++)
{
}

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);
KDL::Frame(KDL::Vector(basePose.x, basePose.y, basePose.z))));

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

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

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

//End Joint
math::Vector3 endPose = mWristJoint->GetAnchor(0);
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 ...
edit retag reopen merge delete