RobotInterface::CalibratorThread::CalibratorThread(yarp::dev::ICalibrator *calibrator,
const std::string &calibratorName,
yarp::dev::DeviceDriver *target,
const std::string &targetName,
RobotInterface::CalibratorThread::Action action) :
mPriv(new Private(this))
{
YARP_ASSERT(calibrator);
YARP_ASSERT(target);
YARP_ASSERT(action == ActionCalibrate || action == ActionPark);
mPriv->calibrator = calibrator;
mPriv->calibratorName = calibratorName;
mPriv->target = target;
mPriv->targetName = targetName;
mPriv->action = action;
}
void TorqueEstimationTree::TorqueEstimationConstructor(KDL::Tree & icub_kdl,
std::vector<kdl_format_io::FTSensorData> ft_sensors,
std::vector<std::string> dof_serialization,
std::vector<std::string> ft_serialization,
yarp::sig::Vector & q_min_yarp, yarp::sig::Vector & q_max_yarp,
std::string fixed_link, unsigned int verbose)
{
std::vector< std::string > ft_names(ft_serialization.size());
std::vector<KDL::Frame> child_sensor_transforms(ft_serialization.size());
KDL::Frame kdlFrame;
for(std::size_t serialization_id=0; serialization_id < ft_serialization.size(); serialization_id++)
{
if( 0 == ft_sensors.size() )
{
std::cerr << "[ERR] TorqueEstimationTree: ft sensor " << ft_serialization[serialization_id] << " not found in model file." << std::endl;
assert(false);
return;
}
for(std::size_t ft_sens=0; ft_sens < ft_sensors.size(); ft_sens++ )
{
std::string ft_sens_name = ft_sensors[ft_sens].reference_joint;
std::size_t ft_sens_id;
if( ft_serialization[serialization_id] == ft_sens_name)
{
ft_sens_id = serialization_id;
ft_names[ft_sens_id] = ft_sens_name;
// \todo TODO FIXME properly address also parent and child cases
// and measure_direction
if( ft_sensors[ft_sens].frame == kdl_format_io::FTSensorData::SENSOR_FRAME )
{
child_sensor_transforms[ft_sens_id] = KDL::Frame(ft_sensors[ft_sens].sensor_pose.M);
}
else
{
child_sensor_transforms[ft_sens_id] = KDL::Frame::Identity();
}
break;
}
if( ft_sens == ft_sensors.size() -1 )
{
std::cerr << "[ERR] TorqueEstimationTree: ft sensor " << ft_sens_name << " not found in model file." << std::endl;
assert(false);
return;
}
}
}
std::cerr << "[INFO] TorqueEstimationTree constructor: loaded urdf with " << dof_serialization.size()
<< "dofs and " << ft_names.size() << " fts ( " << ft_serialization.size() << ") " << std::endl;
//Define an explicit serialization of the links and the DOFs of the iCub
//The DOF serialization done in icub_kdl construction is ok
KDL::CoDyCo::TreeSerialization serial = KDL::CoDyCo::TreeSerialization(icub_kdl);
//Setting a custom dof serialization (\todo TODO FIXME : quite an hack, substitute with proper)
if( dof_serialization.size() != 0 )
{
YARP_ASSERT(dof_serialization.size() == serial.getNrOfDOFs());
for(std::size_t dof=0; dof < dof_serialization.size(); dof++)
{
std::string dof_string = dof_serialization[dof];
YARP_ASSERT(serial.getDOFID(dof_string) != -1);
YARP_ASSERT(serial.getJunctionID(dof_string) != -1);
}
for(std::size_t dof=0; dof < dof_serialization.size(); dof++)
{
std::string dof_string = dof_serialization[dof];
std::cout << "[DEBUG] TorqueEstimationTree: Setting id of dof " << dof_string << " to " << dof << std::endl;
serial.setDOFNameID(dof_string, (int)dof);
serial.setJunctionNameID(dof_string, (int)dof);
}
}
std::string imu_link_name = "imu_frame";
if( fixed_link != "" )
{
imu_link_name = fixed_link;
}
this->constructor(icub_kdl,ft_names,imu_link_name,serial);
std::cerr << "[INFO] TorqueEstimationTree constructor: loaded urdf with " << this->getNrOfDOFs()
<< "dofs and " << ft_names.size() << " fts ( " << ft_serialization.size() << ") " << std::endl;
assert(this->getNrOfDOFs() > 0);
this->setJointBoundMin(q_min_yarp);
this->setJointBoundMax(q_max_yarp);
return;
}