Skip to content

Commit

Permalink
Fix the way we retrieve the transform for the sensors
Browse files Browse the repository at this point in the history 
This allows to export a sensor with a frame not specified in exportedFrames. It fixes #77.
  • Loading branch information
@Nicogene
Nicogene committed Feb 21, 2024
1 parent 3d72a45 commit 5227483
Show file tree
Hide file tree
Showing 3 changed files with 68 additions and 20 deletions.
16 changes: 14 additions & 2 deletions src/creo2urdf/include/creo2urdf/Sensorizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,19 +43,25 @@ struct Sensorizer {

/**
* @brief Assigns a 3D transform to a force/torque sensor based on provided information.
* @param exported_frame_info_map A map of exported frame information.
* @param link_info_map A map of link information.
* @param joint_info_map A map of joint information.
* @param scale The scale for the position part of the 3D transform.
*/
void assignTransformToFTSensor(const std::map<std::string, LinkInfo>& link_info_map,
void assignTransformToFTSensor(const std::map<std::string, ExportedFrameInfo>& exported_frame_info_map,
const std::map<std::string, LinkInfo>& link_info_map,
const std::map<std::string, JointInfo>& joint_info_map,
const std::array<double, 3> scale);

/**
* @brief Assigns a 3D transform to all sensors based on provided information.
* @param exported_frame_info_map A map of exported frame information.
* @param link_info_map A map of link information.
* @param scale The scale for the position part of the 3D transform.
*/
void assignTransformToSensors(const std::map<std::string, ExportedFrameInfo>& exported_frame_info_map);
void assignTransformToSensors(const std::map<std::string, ExportedFrameInfo>& exported_frame_info_map,
const std::map<std::string, LinkInfo>& link_info_map,
const std::array<double, 3> scale);

/**
* @brief Builds a vector of XML trees as strings for force/torque sensors,
Expand Down Expand Up @@ -84,6 +90,12 @@ struct Sensorizer {
* @brief Vector containing information about general sensors.
*/
std::vector<SensorInfo> sensors;

/**
* @brief YAML node containing sensor configuration.
*/
YAML::Node m_config;

};


Expand Down
4 changes: 2 additions & 2 deletions src/creo2urdf/src/Creo2Urdf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -263,9 +263,9 @@ void Creo2Urdf::OnCommand() {
}

// Assign the transforms for the sensors
sensorizer.assignTransformToSensors(exported_frame_info_map);
sensorizer.assignTransformToSensors(exported_frame_info_map, link_info_map, scale);
// Assign the transforms for the ft sensors
sensorizer.assignTransformToFTSensor(link_info_map, joint_info_map, scale);
sensorizer.assignTransformToFTSensor(exported_frame_info_map, link_info_map, joint_info_map, scale);

// Let's add sensors and ft sensors frames

Expand Down
68 changes: 52 additions & 16 deletions src/creo2urdf/src/Sensorizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@

void Sensorizer::readSensorsFromConfig(const YAML::Node & config)
{
m_config = config;
if (!config["sensors"].IsDefined())
return;

Expand Down Expand Up @@ -102,25 +103,32 @@ void Sensorizer::readFTSensorsFromConfig(const YAML::Node& config)

}

void Sensorizer::assignTransformToFTSensor(const std::map<std::string, LinkInfo>& link_info_map, const std::map<std::string, JointInfo>& joint_info_map, const std::array<double, 3> scale)
void Sensorizer::assignTransformToFTSensor(const std::map<std::string, ExportedFrameInfo>& exported_frame_info_map,const std::map<std::string, LinkInfo>& link_info_map, const std::map<std::string, JointInfo>& joint_info_map, const std::array<double, 3> scale)
{
// Iterate over all sensors
for (auto& f : ft_sensors)
{
JointInfo j_info = joint_info_map.at(f.second.frameName);
if (exported_frame_info_map.find(f.second.frameName) != exported_frame_info_map.end())
{
// If the frame used is in the exported frames map, use the transform from there
f.second.child_link_H_sensor = exported_frame_info_map.at(f.second.frameName).linkFrame_H_additionalFrame * exported_frame_info_map.at(f.second.frameName).additionalTransformation;
}
else {
JointInfo j_info = joint_info_map.at(f.second.frameName);

LinkInfo parent_l_info = link_info_map.at(j_info.parent_link_name);
LinkInfo child_l_info = link_info_map.at(j_info.child_link_name);
LinkInfo parent_l_info = link_info_map.at(j_info.parent_link_name);
LinkInfo child_l_info = link_info_map.at(j_info.child_link_name);

auto parent_csys_H_sensor = (getTransformFromPart(parent_l_info.modelhdl, f.second.frameName, scale)).second;
auto parent_csys_H_parent_link = (getTransformFromPart(parent_l_info.modelhdl, parent_l_info.link_frame_name, scale)).second;
// This transform is used for exporting the ft frame
f.second.parent_link_H_sensor = parent_csys_H_parent_link.inverse() * parent_csys_H_sensor;
auto parent_csys_H_sensor = (getTransformFromPart(parent_l_info.modelhdl, f.second.frameName, scale)).second;
auto parent_csys_H_parent_link = (getTransformFromPart(parent_l_info.modelhdl, parent_l_info.link_frame_name, scale)).second;
// This transform is used for exporting the ft frame
f.second.parent_link_H_sensor = parent_csys_H_parent_link.inverse() * parent_csys_H_sensor;

auto child_csys_H_sensor = (getTransformFromPart(child_l_info.modelhdl, f.second.frameName, scale)).second;
auto child_csys_H_child_link = (getTransformFromPart(child_l_info.modelhdl, child_l_info.link_frame_name, scale)).second;
// This transform is used for defining the pose of the ft sensor
f.second.child_link_H_sensor = child_csys_H_child_link.inverse() * child_csys_H_sensor;
auto child_csys_H_sensor = (getTransformFromPart(child_l_info.modelhdl, f.second.frameName, scale)).second;
auto child_csys_H_child_link = (getTransformFromPart(child_l_info.modelhdl, child_l_info.link_frame_name, scale)).second;
// This transform is used for defining the pose of the ft sensor
f.second.child_link_H_sensor = child_csys_H_child_link.inverse() * child_csys_H_sensor;
}
}
}

Expand Down Expand Up @@ -218,18 +226,46 @@ std::vector<std::string> Sensorizer::buildFTXMLBlobs()
return ft_xml_blobs;
}

void Sensorizer::assignTransformToSensors(const std::map<std::string, ExportedFrameInfo>& exported_frame_info_map)
void Sensorizer::assignTransformToSensors(const std::map<std::string, ExportedFrameInfo>& exported_frame_info_map, const std::map<std::string, LinkInfo>& link_info_map, const std::array<double, 3> scale)
{
for (auto& s : sensors)
{

if (exported_frame_info_map.find(s.frameName) != exported_frame_info_map.end())
{
s.transform = exported_frame_info_map.at(s.frameName).linkFrame_H_additionalFrame;
// If the frame used is in the exported frames map, use the transform from there
s.transform = exported_frame_info_map.at(s.frameName).linkFrame_H_additionalFrame * exported_frame_info_map.at(s.frameName).additionalTransformation;
}
else
{
printToMessageWindow(s.sensorName + ": " + s.frameName + " was not found", c2uLogLevel::WARN);
// Otherwise let's try to compute the transform
bool ret = false;
iDynTree::Transform csys_H_additionalFrame{ iDynTree::Transform::Identity() };
iDynTree::Transform csys_H_linkFrame{ iDynTree::Transform::Identity() };
iDynTree::Transform linkFrame_H_additionalFrame{ iDynTree::Transform::Identity() };
std::string cad_link_name = "";
for (auto& rename : m_config["rename"])
{
if (rename.second.Scalar() == s.linkName)
{
cad_link_name = rename.first.Scalar();
break;
}
}
auto link_info = link_info_map.at(cad_link_name);
std::tie(ret, csys_H_additionalFrame) = getTransformFromPart(link_info.modelhdl, s.frameName, scale);
if (!ret)
{
printToMessageWindow("Unable to get the transform for " + s.frameName, c2uLogLevel::WARN);
continue;
}
std::tie(ret, csys_H_linkFrame) = getTransformFromPart(link_info.modelhdl, link_info.link_frame_name, scale);
if (!ret)
{
printToMessageWindow("Unable to get the transform for " + link_info.link_frame_name, c2uLogLevel::WARN);
continue;
}
linkFrame_H_additionalFrame = csys_H_linkFrame.inverse() * csys_H_additionalFrame;
s.transform = linkFrame_H_additionalFrame;
}
}
}
Expand Down

0 comments on commit 5227483

Please sign in to comment.