iDynTree::SensorsList generateSensorsTree(const KDL::CoDyCo::UndirectedTree & undirected_tree,
const std::vector<std::string> & ft_names,
const std::vector<bool> & is_measure_direction_child_to_parent)
{
iDynTree::SensorsList sensors_tree;
for(size_t i=0; i < ft_names.size(); i++ )
{
//Creating a new ft sensor to be added in the ft sensors structure
iDynTree::SixAxisForceTorqueSensor new_sens;
if( undirected_tree.getJunction(ft_names[i]) != undirected_tree.getInvalidJunctionIterator() )
{
//Set the sensor name (for the time being equal to the junction name)
new_sens.setName(ft_names[i]);
//Set the junction name
new_sens.setParentJoint(ft_names[i]);
int junction_index = undirected_tree.getJunction(ft_names[i])->getJunctionIndex();
new_sens.setParentJointIndex(junction_index);
KDL::CoDyCo::JunctionMap::const_iterator junct_it = undirected_tree.getJunction(ft_names[i]);
int parent_index = junct_it->getParentLink()->getLinkIndex();
int child_index = junct_it->getChildLink()->getLinkIndex();
std::string parent_link_name = junct_it->getParentLink()->getName();
std::string child_link_name = junct_it->getChildLink()->getName();
if( is_measure_direction_child_to_parent[i] )
{
new_sens.setAppliedWrenchLink(parent_index);
}
else
{
new_sens.setAppliedWrenchLink(child_index);
}
// Currently we support only the case where the ft sensor frame is equal
// to the child link frame
new_sens.setSecondLinkSensorTransform(child_index,iDynTree::Transform::Identity());
new_sens.setSecondLinkName(child_link_name);
// Then, the parent_link_H_sensor transform is simply parent_link_H_child_link transform
KDL::Frame parent_link_H_sensor = junct_it->pose(0.0,false);
new_sens.setFirstLinkSensorTransform(parent_index,iDynTree::ToiDynTree(parent_link_H_sensor));
new_sens.setFirstLinkName(parent_link_name);
}
else
{
std::cerr << "[ERR] DynTree::generateSensorsTree: problem generating sensor for ft "
<< ft_names[i] << std::endl;
assert(false);
}
int ret = sensors_tree.addSensor(new_sens);
assert(ret == i);
}
return sensors_tree;
}
void assertConsistency(std::string modelName)
{
std::cerr << "DynamicsComputationsDynTreeConsistencyUnitTest running on model "
<< modelName << std::endl;
iDynTree::HighLevel::DynamicsComputations dynComp;
iCub::iDynTree::DynTree dynTree;
bool ok = dynComp.loadRobotModelFromFile(modelName);
ok = ok && dynTree.loadURDFModel(modelName);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_DOUBLE(dynTree.getNrOfDOFs(),dynComp.getNrOfDegreesOfFreedom());
ASSERT_EQUAL_DOUBLE(dynTree.getNrOfFrames(),dynComp.getNrOfFrames());
// Load model with the same joint ordering of the DynTree
iDynTree::ModelLoader mdlLoader;
ok = mdlLoader.loadModelFromFile(modelName);
ASSERT_IS_TRUE(ok);
std::cerr << "Loaded model " << std::endl;
std::vector<std::string> consideredJoints;
KDL::CoDyCo::UndirectedTree undirectedTree = dynTree.getKDLUndirectedTree();
for(int i=0; i < undirectedTree.getNrOfJunctions(); i++)
{
std::string jointName = undirectedTree.getJunction(i)->getName();
// If the joint belong to the new model, include it in the considered joints
if( mdlLoader.model().isJointNameUsed(jointName) )
{
consideredJoints.push_back(jointName);
}
}
iDynTree::ModelLoader mdlLoaderReduced;
ok = mdlLoaderReduced.loadReducedModelFromFullModel(mdlLoader.model(),consideredJoints);
ASSERT_IS_TRUE(ok);
iDynTree::KinDynComputations kinDynComp;
ok = kinDynComp.loadRobotModel(mdlLoaderReduced.model());
ASSERT_IS_TRUE(ok);
std::cerr << "Loaded model in kinDynComp" << std::endl;
Vector6 baseAccKinDyn;
JointDOFsDoubleArray jntAccKinDyn;
setRandomState(dynComp,dynTree,kinDynComp,baseAccKinDyn,jntAccKinDyn);
std::cerr << "Test transforms " << std::endl;
testTransformsConsistency(dynComp,dynTree,kinDynComp);
std::cerr << "Test jacob " << std::endl;
testJacobianConsistency(dynComp,dynTree,kinDynComp);
testRegressorConsistency(dynComp,dynTree);
testCOMConsistency(dynComp,dynTree,kinDynComp);
testMassMatrixConsistency(dynComp,dynTree,kinDynComp);
testInverseDynamicsConsistency(dynComp,kinDynComp,baseAccKinDyn,jntAccKinDyn);
}
bool simpleLeggedOdometry::init(KDL::CoDyCo::UndirectedTree & undirected_tree,
const int initial_world_frame_position_index,
const int initial_fixed_link_index)
{
if( odometry_model )
{
delete odometry_model;
odometry_model=0;
}
odometry_model = new iCub::iDynTree::DynTree(undirected_tree.getTree(),undirected_tree.getSerialization());
bool ok = reset(initial_world_frame_position_index,initial_fixed_link_index);
return ok;
}
bool simpleLeggedOdometry::init(KDL::CoDyCo::UndirectedTree & undirected_tree,
const std::string& initial_world_frame_position,
const std::string& initial_fixed_link)
{
int initial_world_frame_position_index = undirected_tree.getLink(initial_world_frame_position)->getLinkIndex();
int initial_fixed_link_index = undirected_tree.getLink(initial_fixed_link)->getLinkIndex();;
if( initial_fixed_link_index < 0 ||
initial_world_frame_position_index < 0 )
{
return false;
}
return init(undirected_tree,initial_world_frame_position_index,initial_fixed_link_index);
}
/**
*
* @param _reverse_direction if true, reverse the direction of the regressor (root to joint instead of leaf to joint) default:false
*/
torqueRegressor(const KDL::CoDyCo::UndirectedTree & _undirected_tree,
const iDynTree::SensorsList & _sensors_tree,
const std::vector<int> & _linkIndeces2regrCols,
const std::string & dof_name,
const bool _reverse_direction = false,
const std::vector<bool> & _activated_ft_sensors=std::vector< bool>(0),
const bool _consider_ft_offset=false,
const bool _verbose=true
)
: p_undirected_tree(&_undirected_tree),
p_sensors_tree(&_sensors_tree),
linkIndeces2regrCols(_linkIndeces2regrCols),
torque_dof(dof_name),
reverse_direction(_reverse_direction),
activated_ft_sensors(_activated_ft_sensors),
consider_ft_offset(_consider_ft_offset),
subtree_links_indices(0),
verbose(_verbose),
NrOfRealLinks_subtree(0)
{
assert(linkIndeces2regrCols.size() == p_undirected_tree->getNrOfLinks());
NrOfRealLinks_subtree = 0;
for(int ll=0; ll < (int)linkIndeces2regrCols.size(); ll++ ) { if( linkIndeces2regrCols[ll] != -1 ) { NrOfRealLinks_subtree++; } }
assert(NrOfRealLinks_subtree >= 0);
assert(NrOfRealLinks_subtree <= (int)linkIndeces2regrCols.size());
}
/**
* Constructor for base dynamics regressor
*
*/
baseDynamicsRegressor(const KDL::CoDyCo::UndirectedTree & _undirected_tree,
const std::vector<int> & _linkIndeces2regrCols,
bool _verbose=true):
p_undirected_tree(&_undirected_tree),
linkIndeces2regrCols(_linkIndeces2regrCols),
verbose(_verbose)
{
assert(linkIndeces2regrCols.size() == p_undirected_tree->getNrOfLinks());
NrOfRealLinks_subtree = 0;
for(int ll=0; ll < (int)linkIndeces2regrCols.size(); ll++ ) { if( linkIndeces2regrCols[ll] != -1 ) { NrOfRealLinks_subtree++; } }
assert(NrOfRealLinks_subtree >= 0);
assert(NrOfRealLinks_subtree <= (int)linkIndeces2regrCols.size());
}
/**
* Constructor for subtree articulated dynamics regressor
*
* @param _subtree_leaf_links the list of name of the leaf links of the considered subtree
*/
subtreeBaseDynamicsRegressor(const KDL::CoDyCo::UndirectedTree & _undirected_tree,
const iDynTree::SensorsList & _sensors_tree,
const std::vector<int> & _linkIndices2regrCols,
std::vector< std::string> _subtree_leaf_links=std::vector< std::string>(0),
const bool _consider_ft_offset=false,
bool _verbose=true):
p_undirected_tree(&_undirected_tree),
p_sensors_tree(&_sensors_tree),
linkIndices2regrCols(_linkIndices2regrCols),
subtree_leaf_links(_subtree_leaf_links),
consider_ft_offset(_consider_ft_offset),
subtree_links_indices(0),
verbose(_verbose),
NrOfRealLinks_subtree(0)
{
assert(linkIndices2regrCols.size() == p_undirected_tree->getNrOfLinks());
NrOfRealLinks_subtree = 0;
for(int ll=0; ll < (int)linkIndices2regrCols.size(); ll++ ) { if( linkIndices2regrCols[ll] != -1 ) { NrOfRealLinks_subtree++; } }
assert(NrOfRealLinks_subtree >= 0);
assert(NrOfRealLinks_subtree <= (int)linkIndices2regrCols.size());
}
bool checkURDF2DHForAGivenChain(const KDL::CoDyCo::UndirectedTree& undirectedTree,
const std::string& base_frame,
const std::string& end_effector_frame)
{
KDL::Chain kdl_chain;
iCub::iKin::iKinLimb ikin_limb;
//cerr << "urdf2dh test testing chain extraction between " << base_frame
// << " and " << end_effector_frame << endl;
KDL::Tree kdl_rotated_tree = undirectedTree.getTree(base_frame);
bool result = kdl_rotated_tree.getChain(base_frame,end_effector_frame,kdl_chain);
if( !result )
{
cerr << "urdf2dh: Impossible to find " << base_frame << " or "
<< end_effector_frame << " in the URDF." << endl;
return false;
}
//
// Convert the chain and the limits to an iKin chain (i.e. DH parameters)
//
KDL::JntArray qmin(kdl_chain.getNrOfJoints()),qmax(kdl_chain.getNrOfJoints());
for(size_t i=0; i < qmin.rows(); i++ ) { qmin(i) = -10.0; qmax(i) = 10.0; }
result = iKinLimbFromKDLChain(kdl_chain,ikin_limb,qmin,qmax,1000);
if( !result )
{
cerr << "urdf2dh: Could not export KDL::Tree to iKinChain" << endl;
return false;
}
return checkChainsAreEqual(kdl_chain,ikin_limb);
}
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;
}
