VVV08 Grasping Group

From Wiki for iCub and Friends
Jump to: navigation, search

Local SVN Repository

Repository for the reaching people (hosted at Alexis' laptop for speed):

Please send me (Alexis) your public ssh key (Usually ~/.ssh/id_rsa.pub or ~/.ssh/id_dsa.pub) (Maybe per email to: maldonad <at> cs.tum.edu), then this will work:

 svn co svn+ssh://vvv08@192.168.1.234/home/vvv08/svn-repo/ vvv-reaching

That will create a directory called vvv-reaching, where we can exchange code more easily


Small example on how to create a CartesianSpace controller

  //Get the current joint positions
  KDL::JntArray q_current=....;
  
  //Calculate the forward kinematics to get the current cartesian position
  KDL::Frame F_current;
  bool succes = KDL::ChainFkSolverPos->JntToCart(q_current,F_current);
  
  //Apply your control-law (this is feedback, Federico uses attraction fields)
  KDL::Frame F_desired = ...;
  KDL:: Twist V_control=K*diff(F_desired , F_current)
  
  //Calculate the inverse kinematics:
  KDL::JntArray q_dot_control(size);
  succes = KDL::ChainIkSolverVel->CartToJnt(q_current,V_control,q_dot_control)
  //Send the joint velocities to the robot
  .....(q_dot_control)

We will try some different approaches for the ChainIkSolverVel (Damped Least Squares, Weighted Damped Leist Squares, some algorithm that takes into account the jointlimits)

Status of implementations in KDL

  • The Damped Least Squares inverse kinematics are implemented you can find it in kdl/src/chainiksolvervel_dls.[hpp/cpp].
    It has an extra method setLambda to set the damping factor.
    See C.W. Wampler "Manipulator inverse kinematic solutions based on vector formulations and damped least-squares methods" IEEE Transactions on Systems, Man and Cybernetics, volume 16, No 1, pp. 93-101, januari/februari 1986 for theoretical explanation.
  • The weighted pseudo inverse kinematics are implemented in kdl/src/chainiksolvervel_wpinv.[hpp/cpp]. It contains two functions setWeightTS/setWeightJS to set the weighting matrices that are taken into account. The setWeightTS can be used to enable/disable or give more importance to Task Space coordinates (X,Y,Z,Rx,Ry,Rz). The setWeightJS can be used to enable/disable or give more importance to Joints.
  • The weighted damped least squares inverse kinematics is implemented in kdl/src/chainiksolvervel_wdls.[hpp/cpp]. This is a combination of the two implementations above. Making it save to use the weighted inverse kinematics near singular positions of the kinematic chain.
  • There is a weighted damped least squares inverse kinematics that takes into account the joint position limits. It does not yet have the setWeightJS function.

Controlling the robot with the Damped Least Squares inverse kinematics

  • Observe how the damping factor doesn't allow the arm reach a singular position, and how the torso moves to reach the final position

Initial position

Intermediate position

Final position

  • Here you can see the VIDEO

Video

New Denavit Hartenberg parameters for the Right and Left Arms

  • There has been discovered some differences on the Denavit Hartenberg parameters from the Matlab files and the actual robot.
  • Here you can find the C++ Code with the corrected Denavit Hartenberg parameters.
  • There were some differences on the simulator, those have been already been solved, just update the code from the repository.

$Description in C++,

// RIGHT ARM // Tx Rx Tz Rz

 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0320000,  M_PI/2.0,  0.0    , 0.0     )));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000,  M_PI/2.0,  0.0    ,-M_PI/2.0)));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(-0.0233647,  M_PI/2.0, -0.14330,-(105.0/180.0)*M_PI)));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000,  M_PI/2.0, -0.10774,-M_PI/2.0)));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000, -M_PI/2.0,  0.00000,-M_PI/2.0)));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000, -M_PI/2.0, -0.15228,-(105.0/180.0)*M_PI)));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0150000,  M_PI/2.0,  0.0    , 0.0     )));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000,  M_PI/2.0, -0.13730,-M_PI/2.0)));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000,  M_PI/2.0,  0.00000, M_PI/2.0)));
 rightArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0625000,  0.0     ,  0.01600, M_PI    )));

//LEFT ARM // Tx Rx Tz Rz

 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0320000,  M_PI/2.0,  0.0    , 0.0     )));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000,  M_PI/2.0,  0.0    ,-M_PI/2.0)));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0233647, -M_PI/2.0, -0.14330, (105.0/180.0)*M_PI)));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000, -M_PI/2.0,  0.10774, M_PI/2.0)));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000,  M_PI/2.0,  0.00000,-M_PI/2.0)));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000, -M_PI/2.0,  0.15228, (75.0/180.0)*M_PI)));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH(-0.0150000,  M_PI/2.0,  0.0    , 0.0     )));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000,  M_PI/2.0,  0.13730,-M_PI/2.0)));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0000000,  M_PI/2.0,  0.00000, M_PI/2.0)));
 leftArmChain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame().DH( 0.0625000,  0.0     , -0.01600, 0.0     )));