KDL::Wrenches idynChainGet_usingKDL_aux(iCub::iDyn::iDynChain & idynChain, iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
yarp::sig::Vector q,dq,ddq;
q = idynChain.getAng();
dq = idynChain.getDAng();
ddq = idynChain.getD2Ang();
KDL::Chain kdlChain;
std::vector<std::string> la_joints;
la_joints.push_back("left_shoulder_pitch");
la_joints.push_back("left_shoulder_roll");
la_joints.push_back("left_arm_ft_sensor");
la_joints.push_back("left_shoulder_yaw");
la_joints.push_back("left_elbow");
la_joints.push_back("left_wrist_prosup");
la_joints.push_back("left_wrist_pitch");
la_joints.push_back("left_wrist_yaw");
idynSensorChain2kdlChain(idynChain,idynSensor,kdlChain,la_joints,la_joints);
std::cout << kdlChain << std::endl;
int nj = idynChain.getN();
//cout << "idynChainGet_usingKDL_aux with sensor" << " nrJoints " << kdlChain.getNrOfJoints() << " nrsegments " << kdlChain.getNrOfSegments() << endl;
assert(nj==kdlChain.getNrOfJoints());
assert(nj+1==kdlChain.getNrOfSegments());
KDL::JntArray jointpositions = KDL::JntArray(nj);
KDL::JntArray jointvel = KDL::JntArray(nj);
KDL::JntArray jointacc = KDL::JntArray(nj);
KDL::JntArray torques = KDL::JntArray(nj);
for(unsigned int i=0;i<nj;i++){
jointpositions(i)=q[i];
jointvel(i) = dq[i];
jointacc(i) = ddq[i];
}
KDL::Wrenches f_ext(nj+1);
KDL::Wrenches f_int(nj+1);
KDL::Vector grav_kdl;
idynVector2kdlVector(idynChain.getH0().submatrix(0,2,0,2).transposed()*ddp0,grav_kdl);
KDL::ChainIdSolver_RNE_IW neSolver = KDL::ChainIdSolver_RNE_IW(kdlChain,-grav_kdl);
int ret = neSolver.CartToJnt_and_internal_wrenches(jointpositions,jointvel,jointacc,f_ext,torques,f_int);
assert(ret >= 0);
return f_int;
}
iDynTree::Wrench simulateFTSensorFromKinematicState(const KDL::CoDyCo::UndirectedTree & icub_undirected_tree,
const KDL::JntArray & q,
const KDL::JntArray & dq,
const KDL::JntArray & ddq,
const KDL::Twist & base_velocity,
const KDL::Twist & base_acceleration,
const std::string ft_sensor_name,
const iDynTree::SensorsList & sensors_tree
)
{
// We can try to simulate the same sensor with the usual inverse dynamics
KDL::CoDyCo::Traversal traversal;
icub_undirected_tree.compute_traversal(traversal);
std::vector<KDL::Twist> v(icub_undirected_tree.getNrOfLinks());
std::vector<KDL::Twist> a(icub_undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> f(icub_undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> f_gi(icub_undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> f_ext(icub_undirected_tree.getNrOfLinks(),KDL::Wrench::Zero());
KDL::JntArray torques(icub_undirected_tree.getNrOfDOFs());
KDL::Wrench base_force;
KDL::CoDyCo::rneaKinematicLoop(icub_undirected_tree,
q,dq,ddq,traversal,base_velocity,base_acceleration,
v,a,f_gi);
KDL::CoDyCo::rneaDynamicLoop(icub_undirected_tree,q,traversal,f_gi,f_ext,f,torques,base_force);
unsigned int sensor_index;
sensors_tree.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_sensor_name,sensor_index);
std::cout << ft_sensor_name << " has ft index " << sensor_index << std::endl;
iDynTree::SixAxisForceTorqueSensor * p_sens
= (iDynTree::SixAxisForceTorqueSensor *) sensors_tree.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,sensor_index);
iDynTree::Wrench simulate_measurement;
KDL::CoDyCo::Regressors::simulateMeasurement_sixAxisFTSensor(traversal,f,p_sens,simulate_measurement);
return simulate_measurement;
}
yarp::sig::Vector idynChainGetTorques_usingKDL(iCub::iDyn::iDynChain & idynChain,iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
yarp::sig::Vector q,dq,ddq;
q = idynChain.getAng();
dq = idynChain.getDAng();
ddq = idynChain.getD2Ang();
KDL::Chain kdlChain;
idynSensorChain2kdlChain(idynChain,idynSensor,kdlChain);
int nj = idynChain.getN();
KDL::JntArray jointpositions = KDL::JntArray(nj);
KDL::JntArray jointvel = KDL::JntArray(nj);
KDL::JntArray jointacc = KDL::JntArray(nj);
KDL::JntArray torques = KDL::JntArray(nj);
for(unsigned int i=0;i<nj;i++){
jointpositions(i)=q[i];
jointvel(i) = dq[i];
jointacc(i) = ddq[i];
}
KDL::Wrenches f_ext(nj+1);
KDL::Wrenches f_int(nj+1);
KDL::Vector grav_kdl;
idynVector2kdlVector(idynChain.getH0().submatrix(0,2,0,2).transposed()*ddp0,grav_kdl);
KDL::ChainIdSolver_RNE_IW neSolver = KDL::ChainIdSolver_RNE_IW(kdlChain,-grav_kdl);
int ret = neSolver.CartToJnt_and_internal_wrenches(jointpositions,jointvel,jointacc,f_ext,torques,f_int);
assert(ret >= 0);
yarp::sig::Vector ret_tau;
kdlJntArray2idynVector(torques,ret_tau);
return ret_tau;
}
/**
* In this example we will perform a classical inverse dynamics on tree structured robot.
* We will load the kinematic and dynamic parameters of the robot from an URDF file.
*/
int main(int argc, char** argv)
{
if(argc != 2)
{
std::cerr << "tree_inverse_dynamics example usage: ./tree_inverse_dynamics robot.urdf" << std::endl;
return EXIT_FAILURE;
}
std::string urdf_file_name = argv[1];
//We are using the kdl_format_io library for loading the URDF file to a KDL::Tree object, but if
//you use ROS you can use the kdl_parser from the robot_model package (but please note that
//there are some open issues with the robot_model kdl_parser : https://github.com/ros/robot_model/pull/66 )
KDL::Tree my_tree;
bool urdf_loaded_correctly = iDynTree::treeFromUrdfFile(urdf_file_name.c_str(),my_tree);
if( !urdf_loaded_correctly )
{
std::cerr << "Could not generate robot model and extract kdl tree" << std::endl;
return EXIT_FAILURE;
}
//We will now create a tree inverse dynamics solver
//This solver performs the classical inverse dynamics
//so there is a link called floating base that is the one for
//which the velocity and the acceleration is specified, and
//where unknown external wrench is assumed applied.
//The floating base is by default the base link of the URDF, but
//can be changed with the changeBase method
KDL::CoDyCo::TreeIdSolver_RNE rne_solver(my_tree);
//The input variables are :
// - Joint positions, velocities, accelerations.
int n_dof = rne_solver.getUndirectedTree().getNrOfDOFs();
KDL::JntArray q_j(n_dof), dq_j(n_dof), ddq_j(n_dof);
// - Floating base velocity and (proper) acceleration.
// The proper acceleration is the sum of the classical and gravitational acceleration,
// i.e. the acceleration that you get reading the output of linear accelerometer.
KDL::Twist v_base, a_base;
// - External wrenches applied to the link. This wrenches are expressed in the local reference frame of the link.
int n_links = rne_solver.getUndirectedTree().getNrOfLinks();
std::vector<KDL::Wrench> f_ext(n_links,KDL::Wrench::Zero());
//The output variables are :
// - Joint torques.
KDL::JntArray torques(n_dof);
// - The base residual wrench (mismatch between external wrenches in input and the model).
KDL::Wrench f_base;
//We populate the input variables with random data
srand(0);
for(int dof=0; dof < n_dof; dof++)
{
q_j(dof) = fRand(-10,10);
dq_j(dof) = fRand(-10,10);
ddq_j(dof) = fRand(-10,10);
}
//We fill also the linear part of the base velocity
//but please note that the dynamics is indipendent from it (Galilean relativity)
for(int i=0; i < 6; i++ )
{
v_base[i] = fRand(-10,10);
a_base[i] = fRand(-10,10);
}
//For setting the input wrenches, we first get the index for the link for which we want to set the external wrench.
//In this example we assume that we have measures for the input wrenches at the four end effectors (two hands, two legs)
//We use the names defined in REP 120 ( http://www.ros.org/reps/rep-0120.html ) for end effector frames
//but please change the names if you want to use a different set of links
int r_gripper_id = rne_solver.getUndirectedTree().getLink("r_gripper")->getLinkIndex();
int l_gripper_id = rne_solver.getUndirectedTree().getLink("l_gripper")->getLinkIndex();
int r_sole_id = rne_solver.getUndirectedTree().getLink("r_sole")->getLinkIndex();
int l_sole_id = rne_solver.getUndirectedTree().getLink("l_sole")->getLinkIndex();
for(int i=0; i < 6; i++ )
{
f_ext[r_gripper_id][i] = fRand(-10,10);
f_ext[l_gripper_id][i] = fRand(-10,10);
f_ext[r_sole_id][i] = fRand(-10,10);
f_ext[l_sole_id][i] = fRand(-10,10);
}
//Now that we have the input, we can compute the inverse dynamics
int inverse_dynamics_status = rne_solver.CartToJnt(q_j,dq_j,ddq_j,v_base,a_base,f_ext,torques,f_base);
if( inverse_dynamics_status != 0 )
{
std::cerr << "There was an error in the inverse dynamics computations" << std::endl;
return EXIT_FAILURE;
}
std::cout << "Torque computed successfully. " << std::endl;
std::cout << "Computed torques : " << std::endl;
for(int dof=0; dof < n_dof; dof++ )
{
std::cout << torques(dof) << " ";
}
std::cout << std::endl;
return EXIT_SUCCESS;
}
int f_def(void) {
f_ext();
f_deferred();
return g_com + g_def + g_ext + g_deferred[0];
}
