VVV08 Grasping Group
From Wiki for RobotCub and Friends
Contents |
[edit]
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
[edit]
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)
[edit]
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.
[edit]
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
- Here you can see the VIDEO
[edit]
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 )));
