SensorsMeasurements::SensorsMeasurements(const SensorsList &sensorsList)
{
this->pimpl = new SensorsMeasurementsPrivateAttributes;
this->pimpl->SixAxisFTSensorsMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE));
this->pimpl->AccelerometerMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::ACCELEROMETER));
this->pimpl->GyroscopeMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::GYROSCOPE));
}
bool SensorsMeasurements::resize(const SensorsList &sensorsList)
{
Wrench zeroWrench;zeroWrench.zero();
LinAcceleration zeroLinAcc;zeroLinAcc.zero();
AngVelocity zeroAngVel;zeroAngVel.zero();
this->pimpl->SixAxisFTSensorsMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE),zeroWrench);
this->pimpl->AccelerometerMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::ACCELEROMETER),zeroLinAcc);
this->pimpl->GyroscopeMeasurements.resize(sensorsList.getNrOfSensors(iDynTree::GYROSCOPE),zeroAngVel);
return true;
}
void SensorsList::constructor(const SensorsList& other)
{
this->pimpl = new SensorsListPimpl();
this->pimpl->VecSensors.resize(NR_OF_SENSOR_TYPES,std::vector<Sensor *>(0));
this->pimpl->NamesSensors.resize(NR_OF_SENSOR_TYPES);
for(int sens_type = 0; sens_type < NR_OF_SENSOR_TYPES; sens_type++ )
{
for(unsigned int sens = 0; sens < other.getNrOfSensors((SensorType)sens_type); sens++ )
{
this->pimpl->VecSensors[sens_type].push_back(other.pimpl->VecSensors[sens_type][sens]->clone());
std::string sensor_name = other.getSensor((SensorType)sens_type,sens)->getName();
this->pimpl->NamesSensors[sens_type].insert(std::pair<std::string,int>(sensor_name,sens));
}
}
}
void Generic1DOFJointSensorGroup::SetSensorsList(const SensorsList& list){
mSensorList.clear();
mSensorCount = 0;
for(unsigned int i=0;i<list.size();i++){
Generic1DOFJointSensor *sensor = Generic1DOFJointSensor::Cast(list[i]);
if(sensor){
mSensorList.push_back(sensor);
mSensorCount ++;
}
}
mPosition.Resize(mSensorCount,false);
mVelocity.Resize(mSensorCount,false);
mAcceleration.Resize(mSensorCount,false);
mTorque.Resize(mSensorCount,false);
mLimits[0].Resize(mSensorCount,false);
mLimits[1].Resize(mSensorCount,false);
mPosition.Zero();
mVelocity.Zero();
mAcceleration.Zero();
mTorque.Zero();
mLimits[0].Zero();
mLimits[1].Zero();
}
int Sensor::SensorsListSetFromStream(SensorsList & list,const void* memory){
char* mem = (char*) memory;
for(size_t i=0;i<list.size();i++)
mem += list[i]->SetFromStream(mem);
return ((char*)mem)-((char*)memory);
}
int Sensor::SensorsListStreamSize(SensorsList & list){
int result=0;
for(size_t i=0;i<list.size();i++)
result += list[i]->StreamSize();
return result;
}
bool sensorsFromURDFString(const std::string& urdfXml,
const Model & model,
SensorsList & outputSensors)
{
SensorsList newSensors;
bool parsingSuccessful = true;
/* parse custon urdf */
TiXmlDocument tiUrdfXml;
tiUrdfXml.Parse(urdfXml.c_str());
TiXmlElement* robotXml = tiUrdfXml.FirstChildElement("robot");
// Several sensor types have been proposed for use in URDF,
// see http://wiki.ros.org/urdf/XML/sensor/proposals ,
// but few of them (the one more relevant to dynamics) are supported by iDynTree
// To avoid printing too many warnings, a warning about the ignored sensor
// is printed only if the sensor has a type that is not on the following list
// In this way we can print a warning to catch eventual typos, but avoid flooding
// the user with meaningless warnings if he loads (for example) a model full of cameras
std::vector<std::string> sensorTypesUsedInURDFButNotSupportedInIDynTree;
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("camera");
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("ray");
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("imu");
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("magnetometer");
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("gps");
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("contact");
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("sonar");
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("rfidtag");
sensorTypesUsedInURDFButNotSupportedInIDynTree.push_back("rfid");
// Get all the sensors elements of robot element
for (TiXmlElement* sensorXml = robotXml->FirstChildElement("sensor");
sensorXml; sensorXml = sensorXml->NextSiblingElement("sensor"))
{
if( sensorXml->Attribute("name")==NULL ||
sensorXml->Attribute("type")==NULL )
{
std::cerr << "[ERROR] Sensor name or type specified incorrectly." << std::endl;
parsingSuccessful = false;
return false;
}
// Parse sensors name
std::string sensorName(sensorXml->Attribute("name"));
// Parse sensor type
std::string sensorType(sensorXml->Attribute("type"));
iDynTree::SensorType type;
if(sensorType.compare("accelerometer") == 0)
{
type = ACCELEROMETER;
}
else if(sensorType.compare("gyroscope") == 0)
{
type = GYROSCOPE;
}
else if(sensorType.compare("force_torque") == 0)
{
type = SIX_AXIS_FORCE_TORQUE;
}
else
{
// Print the warning about ignored sensors only if the sensor type is not on the list (to catch typos)
if( std::find(sensorTypesUsedInURDFButNotSupportedInIDynTree.begin(),sensorTypesUsedInURDFButNotSupportedInIDynTree.end(), sensorType)
== sensorTypesUsedInURDFButNotSupportedInIDynTree.end() )
{
std::string errString = "Specified sensor type " + sensorType + " of sensor with name " + sensorName +" is not recognised, this sensor was ignored by the iDynTree sensor parser.";
reportWarning("","sensorsFromURDFString",errString.c_str());
}
continue;
}
// Parse origin element
Transform link_H_pose;
TiXmlElement* originTag = sensorXml->FirstChildElement("origin");
if( originTag )
{
std::string rpyText(originTag->Attribute("rpy"));
std::string xyzText(originTag->Attribute("xyz"));
std::vector<std::string> rpyElems;
std::vector<std::string> xyzElems;
splitString(rpyText,rpyElems);
splitString(xyzText,xyzElems);
if( rpyElems.size() != 3 && xyzElems.size() !=3 )
{
std::cerr<<"[ERROR] Pose attribute for sensor " << sensorName << " specified incorrectly, parsing failed.";
parsingSuccessful = false;
return false;
}
double roll = atof(rpyElems[0].c_str());
double pitch = atof(rpyElems[1].c_str());
double yaw = atof(rpyElems[2].c_str());
iDynTree::Rotation rot = iDynTree::Rotation::RPY(roll,pitch,yaw);
double x = atof(xyzElems[0].c_str());
double y = atof(xyzElems[1].c_str());
double z = atof(xyzElems[2].c_str());
iDynTree::Position pos(x,y,z);
link_H_pose = iDynTree::Transform(rot,pos);
}
else
{
// default value
link_H_pose = iDynTree::Transform::Identity();
}
// Parse parent element
TiXmlElement* parent = sensorXml->FirstChildElement("parent");
std::string parentLink, parentJoint;
JointIndex parentJointIndex = JOINT_INVALID_INDEX;
LinkIndex parentLinkIndex = LINK_INVALID_INDEX;
if(!parent)
{
std::cerr<< "[ERROR] parent element not found in sensor " << sensorName <<
", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
if(parent->Attribute("link")!= NULL)
{
parentLink = std::string(parent->Attribute("link"));
parentLinkIndex = model.getLinkIndex(parentLink);
if( parentLinkIndex == LINK_INVALID_INDEX )
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " has parent link " << parentLink
<< " but the link is not found on the model, parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
if( type == SIX_AXIS_FORCE_TORQUE )
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " of type force_torque cannot be child of a link "
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
}
else if(parent->Attribute("joint")!=NULL)
{
parentJoint = std::string(parent->Attribute("joint"));
parentJointIndex = model.getJointIndex(parentJoint);
if( parentJointIndex == JOINT_INVALID_INDEX )
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " has parent joint " << parentJoint
<< " but the joint is not found on the model, parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
if( type == GYROSCOPE )
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " of type gyroscope cannot be child of a joint "
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
if( type == ACCELEROMETER )
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " of type accelerometer cannot be child of a joint "
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
}
else
{
std::cerr << "[ERROR] Link or Joint attribute for parent element not found in sensor "
<< sensorName << ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
// Actually allocate the sensors
if( type == SIX_AXIS_FORCE_TORQUE )
{
// parse frame
enum { PARENT_LINK_FRAME ,
CHILD_LINK_FRAME ,
SENSOR_FRAME } frame_type;
enum { PARENT_TO_CHILD,
CHILD_TO_PARENT } measure_direction_type;
TiXmlElement* force_torque = sensorXml->FirstChildElement("force_torque");
if( !force_torque )
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " of type force_torque is missing force_torque element "
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
TiXmlElement* frame_el = force_torque->FirstChildElement("frame");
TiXmlElement* measure_direction_el = force_torque->FirstChildElement("measure_direction");
// parse frame
if( !frame_el || !measure_direction_el )
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " of type force_torque is missing frame or measure_direction element "
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
std::string frame = frame_el->GetText();
if( frame == "child" )
{
frame_type = CHILD_LINK_FRAME;
}
else if( frame == "parent" )
{
frame_type = PARENT_LINK_FRAME;
}
else if( frame == "sensor" )
{
frame_type = SENSOR_FRAME;
}
else
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " of type force_torque has unexpected frame content " << frame
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
std::string measure_direction = measure_direction_el->GetText();
if( measure_direction == "parent_to_child" )
{
measure_direction_type = PARENT_TO_CHILD;
}
else if( measure_direction == "child_to_parent" )
{
measure_direction_type = CHILD_TO_PARENT;
}
else
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " of type force_torque has unexpected measure_direction content " << measure_direction
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
// Get parent and child links of the joint to which the FT sensor is attached
std::string parentLinkName, childLinkName;
bool ok = findJointParentsAndChild(robotXml,parentJoint,parentLinkName,childLinkName);
if( !ok )
{
parsingSuccessful = false;
return false;
}
LinkIndex parentLinkIndex = model.getLinkIndex(parentLinkName);
LinkIndex childLinkIndex = model.getLinkIndex(childLinkName);
if( parentLinkIndex == LINK_INVALID_INDEX ||
childLinkIndex == LINK_INVALID_INDEX )
{
std::cerr<< "[ERROR] link " << parentLinkName << " or " << childLinkName
<< " not found when parsing sensor " << sensorName
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
SixAxisForceTorqueSensor * sensor = new SixAxisForceTorqueSensor();
sensor->setName(sensorName);
sensor->setParentJoint(parentJoint);
sensor->setParentJointIndex(parentLinkIndex);
if( measure_direction_type == PARENT_TO_CHILD )
{
sensor->setAppliedWrenchLink(childLinkIndex);
}
else
{
sensor->setAppliedWrenchLink(parentLinkIndex);
}
// The transform tag is parsed using the Gazebo convention
// For now we assume that the six axis ft sensor is attached to a
// fixed junction. Hence the first/second link to sensor transforms
// are fixed are given by the frame option
iDynTree::Transform parent_link_H_child_link = model.getJoint(parentJointIndex)->getRestTransform(parentLinkIndex,childLinkIndex);
iDynTree::Transform child_link_H_sensor = link_H_pose;
if( frame_type == PARENT_LINK_FRAME )
{
sensor->setFirstLinkSensorTransform(parentLinkIndex,iDynTree::Transform::Identity());
sensor->setSecondLinkSensorTransform(childLinkIndex,parent_link_H_child_link.inverse());
}
else if( frame_type == CHILD_LINK_FRAME )
{
sensor->setFirstLinkSensorTransform(parentLinkIndex,parent_link_H_child_link);
sensor->setSecondLinkSensorTransform(childLinkIndex,iDynTree::Transform::Identity());
}
else
{
assert( frame_type == SENSOR_FRAME );
sensor->setFirstLinkSensorTransform(parentLinkIndex,parent_link_H_child_link*child_link_H_sensor);
sensor->setSecondLinkSensorTransform(childLinkIndex,child_link_H_sensor);
}
sensor->setFirstLinkName(parentLinkName);
sensor->setSecondLinkName(childLinkName);
newSensors.addSensor(*sensor);
delete sensor;
}
else if( type == GYROSCOPE )
{
GyroscopeSensor * sensor = new GyroscopeSensor();
sensor->setLinkSensorTransform(link_H_pose);
sensor->setName(sensorName);
sensor->setParentLink(parentLink);
sensor->setParentLinkIndex(parentLinkIndex);
newSensors.addSensor(*sensor);
delete sensor;
}
else if( type == ACCELEROMETER )
{
AccelerometerSensor * sensor = new AccelerometerSensor();
sensor->setLinkSensorTransform(link_H_pose);
sensor->setName(sensorName);
sensor->setParentLink(parentLink);
sensor->setParentLinkIndex(parentLinkIndex);
newSensors.addSensor(*sensor);
delete sensor;
}
else
{
std::cerr<< "[ERROR] sensor " << sensorName
<< " of unknown type "
<< ", parsing failed." << std::endl;
parsingSuccessful = false;
return false;
}
}
// If the parsing was successful,
// copy the obtained sensors to the output structure
if( parsingSuccessful )
{
outputSensors = newSensors;
}
return parsingSuccessful;
}
int main()
{
// Load data
std::string fileName = getAbsModelPath("icub_sensorised.urdf");
Model fullModel = getModel(fileName);
SensorsList fullSensors = getSensors(fileName);
// Store data for a given FT sensor
DataFT dataFT_l_leg;
dataFT_l_leg.jointName = "l_leg_ft_sensor";
// Links
dataFT_l_leg.parent_linkName = "l_hip_2";
dataFT_l_leg.child_linkName = "l_hip_3";
dataFT_l_leg.grandparent_linkName = "l_hip_1";
dataFT_l_leg.grandchild_linkName = "l_upper_leg";
// Joints
dataFT_l_leg.grandparentToParent_jointName = "l_hip_roll";
dataFT_l_leg.childToGranchild_jointName = "l_hip_yaw";
// Get all the iCub joints
vector<string> fullJoints = get_iCubJointsSensorised();
// Keep only one FT
vector<string> removedJoints;
removedJoints.push_back("r_leg_ft_sensor");
removedJoints.push_back("l_foot_ft_sensor");
removedJoints.push_back("r_foot_ft_sensor");
removedJoints.push_back("l_arm_ft_sensor");
removedJoints.push_back("r_arm_ft_sensor");
vector<string> fullJoints_1FT = removeJoints(fullJoints, removedJoints);
// Setup the experiments
// ---------------------
// 1) The reduced model doesn't lump links which the FT is connected
ExperimentFT test_defaultModel;
test_defaultModel.dataFT = dataFT_l_leg;
test_defaultModel.expectedFirstLinkName = dataFT_l_leg.parent_linkName;
test_defaultModel.expectedSecondLinkName = dataFT_l_leg.child_linkName;
// 2) The reduced model doesn't contain the firstLink
ExperimentFT test_removeFirstLink;
test_removeFirstLink.dataFT = dataFT_l_leg;
test_removeFirstLink.removedJoints.push_back(test_removeFirstLink.dataFT.grandparentToParent_jointName);
test_removeFirstLink.expectedFirstLinkName = dataFT_l_leg.grandparent_linkName;
test_removeFirstLink.expectedSecondLinkName = dataFT_l_leg.child_linkName;
// 3) The reduced model doesn't contain the secondLink
ExperimentFT test_removeSecondLink;
test_removeSecondLink.dataFT = dataFT_l_leg;
test_removeSecondLink.removedJoints.push_back(test_removeSecondLink.dataFT.childToGranchild_jointName);
test_removeSecondLink.expectedFirstLinkName = dataFT_l_leg.parent_linkName;
test_removeSecondLink.expectedSecondLinkName = dataFT_l_leg.child_linkName;
// 3) The reduced model doesn't contain both firstLink and secondLink
ExperimentFT test_removeFirstAndSecondLink;
test_removeFirstAndSecondLink.dataFT = dataFT_l_leg;
test_removeFirstAndSecondLink.removedJoints.push_back(test_removeFirstAndSecondLink.dataFT.grandparentToParent_jointName);
test_removeFirstAndSecondLink.removedJoints.push_back(test_removeFirstAndSecondLink.dataFT.childToGranchild_jointName);
test_removeFirstAndSecondLink.expectedFirstLinkName = dataFT_l_leg.grandparent_linkName;
test_removeFirstAndSecondLink.expectedSecondLinkName = dataFT_l_leg.child_linkName;
// Group all the experiments together
vector<ExperimentFT> experiments;
experiments.push_back(test_defaultModel);
experiments.push_back(test_removeFirstLink);
experiments.push_back(test_removeSecondLink);
experiments.push_back(test_removeFirstAndSecondLink);
for (auto experiment : experiments)
{
bool ok;
Model reducedModel;
SensorsList reducedSensors;
ok = createReducedModelAndSensors(fullModel,
fullSensors,
removeJoints(fullJoints_1FT, experiment.removedJoints),
reducedModel,
reducedSensors);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_DOUBLE(reducedSensors.getNrOfSensors(SIX_AXIS_FORCE_TORQUE), 1);
Sensor* s = reducedSensors.getSensor(SIX_AXIS_FORCE_TORQUE, 0);
ASSERT_IS_TRUE(s != nullptr);
ASSERT_IS_TRUE(s->isValid());
JointSensor* jointSens = dynamic_cast<JointSensor*>(s);
ASSERT_IS_TRUE(jointSens != nullptr);
unique_ptr<SixAxisForceTorqueSensor> sensorCopy;
sensorCopy.reset(static_cast<SixAxisForceTorqueSensor*>(jointSens->clone()));
ASSERT_IS_TRUE(sensorCopy != nullptr);
printFT(*sensorCopy);
cout << endl;
ASSERT_EQUAL_STRING(sensorCopy->getFirstLinkName(), experiment.expectedFirstLinkName);
ASSERT_EQUAL_STRING(sensorCopy->getSecondLinkName(), experiment.expectedSecondLinkName);
// Test transform
// --------------
// Get the transform from the fixed joint.
// It uses the createReducedModel() logic. It is used as ground truth.
Transform parent_H_child;
auto jointFT = reducedModel.getJoint(reducedModel.getJointIndex(experiment.dataFT.jointName));
parent_H_child = jointFT->getRestTransform(jointFT->getFirstAttachedLink(),
jointFT->getSecondAttachedLink());
// Get the transform from the sensor.
// This is calculated by createReducedModelAndSensors() and it is the method under test.
LinkIndex firstLinkIndex = sensorCopy->getFirstLinkIndex();
LinkIndex secondLinkIndex = sensorCopy->getSecondLinkIndex();
Transform firstLink_H_sensorFrame;
Transform secondLink_H_sensorFrame;
ok = true;
ok = ok && sensorCopy->getLinkSensorTransform(firstLinkIndex, firstLink_H_sensorFrame);
ok = ok && sensorCopy->getLinkSensorTransform(secondLinkIndex, secondLink_H_sensorFrame);
ASSERT_IS_TRUE(ok);
ASSERT_EQUAL_TRANSFORM(parent_H_child,
firstLink_H_sensorFrame*secondLink_H_sensorFrame.inverse());
}
return EXIT_SUCCESS;
}
