Databases
StackOverflow2013
Postsdbo
PODetermining Joint Rotations in Gazebo using KDL
 

Note that there are some explanatory texts on larger screens.

plurals
  1. PODetermining Joint Rotations in Gazebo using KDL
    primarykey
    data
    text
    <p>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:</p> <pre><code> //Create a KDL Joint Chain of our Arm Robot KDL::Chain chain; //Base Joint math::Vector3 basePose = mBaseJoint-&gt;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-&gt;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-&gt;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-&gt;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-&gt;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 &lt; jointCount - 1; i++) { jointPositions(i) = mJoints[i]-&gt;GetAngle(0).Radian(); } physics::ModelPtr target = mModel-&gt;GetWorld()-&gt;GetModel("Target"); math::Pose pose = target-&gt;GetWorldPose(); math::Pose myPose = mModel-&gt;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 &lt;&lt; "Calculating..." &lt;&lt; std::endl; bool kinematics_status; kinematics_status = myIk.CartToJnt( jointPositions, cartesianPosition, targetRotation ); gzdbg &lt;&lt; "Final Status: " &lt;&lt; kinematics_status &lt;&lt; std::endl; if(kinematics_status &gt;= 0) { for( int i = 0; i &lt; jointCount - 1; i++ ) { mJoints[i]-&gt;SetAngle(0, targetRotation(i)); } }//Create a KDL Joint Chain of our Arm Robot KDL::Chain chain; //Base Joint math::Vector3 basePose = mBaseJoint-&gt;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-&gt;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-&gt;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-&gt;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-&gt;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 &lt; jointCount - 1; i++) { jointPositions(i) = mJoints[i]-&gt;GetAngle(0).Radian(); } physics::ModelPtr target = mModel-&gt;GetWorld()-&gt;GetModel("Target"); math::Pose pose = target-&gt;GetWorldPose(); math::Pose myPose = mModel-&gt;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 &lt;&lt; "Calculating..." &lt;&lt; std::endl; bool kinematics_status; kinematics_status = myIk.CartToJnt( jointPositions, cartesianPosition, targetRotation ); gzdbg &lt;&lt; "Final Status: " &lt;&lt; kinematics_status &lt;&lt; std::endl; if(kinematics_status &gt;= 0) { for( int i = 0; i &lt; jointCount - 1; i++ ) { mJoints[i]-&gt;SetAngle(0, targetRotation(i)); } } </code></pre>
    singulars
    1. This table or related slice is empty.
    1. This table or related slice is empty.
    1. PTQuestion
    1. This table or related slice is empty.
    1. USChris Flynn
    plurals
    1. This table or related slice is empty.
    1. This table or related slice is empty.
    1. This table or related slice is empty.
    1. This table or related slice is empty.
    1. VO
      singulars
      1. PODetermining Joint Rotations in Gazebo using KDL
      1. This table or related slice is empty.
      1. VTUpMod
    1. COYou might try this question also on http://robotics.stackexchange.com/
      singulars
      1. PODetermining Joint Rotations in Gazebo using KDL
      1. USWildCrustacean
    2. COWhat sensors are you using? Quadrature encoders?
      singulars
      1. PODetermining Joint Rotations in Gazebo using KDL
      1. USmichael b
    3. CONo sensors. It manually sets the joint's rotation angle and the target position is a hard-coded constant for testing
      singulars
      1. PODetermining Joint Rotations in Gazebo using KDL
      1. USChris Flynn
 

Querying!

 
Guidance
A row detail

Detail views are divided into sections. All the information in the data section comes from columns in the selected row. The other sections display data from other, related rows.

Related data can be related in a to-one or a to-many fashion. Captions of data related in a to-many fashion link to a list view showing a filtered view of the table.

Try moving around until you find a non-empty to-many entry and click on the label to get to one. You can move back to the root by clicking on the database name in the header.

SQuiL has stopped working due to an internal error.

If you are curious you may find further information in the browser console, which is accessible through the devtools (F12).

Reload