Skip to content

Commit

Permalink
Commit to be squashed and fixed
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nicogene
Nicogene committed Jun 13, 2024
1 parent 4eaac18 commit 1650c8f
Show file tree
Hide file tree
Showing 3 changed files with 25 additions and 27 deletions.
2 changes: 1 addition & 1 deletion src/creo2urdf/include/creo2urdf/Utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -439,7 +439,7 @@ std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl,
* @param scale The scaling factor for the origin of the child frame
* @return std::pair<bool, iDynTree::Direction> Pair containing a success/failure flag, and the axis direction
*/
std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);
std::tuple<bool, iDynTree::Direction, iDynTree::Transform> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const std::string& link_frame_name, const array<double, 3>& scale);

/**
* @brief Extracts the folder path from a file path.
Expand Down
30 changes: 15 additions & 15 deletions src/creo2urdf/src/Creo2Urdf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -267,8 +267,11 @@ void Creo2Urdf::OnCommand() {
if (joint_info.second.type == JointType::Revolute || joint_info.second.type == JointType::Linear) {

iDynTree::Direction axis;
iDynTree::Position axis_offset;
std::tie(ret, axis, axis_offset) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);
iDynTree::Transform oldChild_H_newChild;
std::tie(ret, axis, oldChild_H_newChild) = getAxisFromPart(parent_model, axis_name, parent_link_frame, scale);


parent_H_child = parent_H_child * oldChild_H_newChild;

if (!ret && warningsAreFatal)
{
Expand All @@ -283,32 +286,29 @@ void Creo2Urdf::OnCommand() {

auto urdf_child_link_name = getRenameElementFromConfig(child_link_name);

printToMessageWindow("joint " + urdf_child_link_name);

// THIS IS NOT HAVING EFFECT
/*auto link_H_collision_solid_shape = idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();
auto link_H_collision_solid_shape = idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();
auto link_H_visual_solid_shape = idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();

link_H_collision_solid_shape.setPosition(link_H_collision_solid_shape.getPosition() - axis_offset);
link_H_visual_solid_shape.setPosition(link_H_visual_solid_shape.getPosition() - axis_offset);
printToMessageWindow("BEFORE link_H_collision_solid_shape " + link_H_visual_solid_shape.toString());

idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(link_H_collision_solid_shape);
idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(link_H_visual_solid_shape);
idyn_model.collisionSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(oldChild_H_newChild.inverse() * link_H_collision_solid_shape);
idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->setLink_H_geometry(oldChild_H_newChild.inverse() * link_H_visual_solid_shape);

link_H_visual_solid_shape = idyn_model.visualSolidShapes().getLinkSolidShapes()[idyn_model.getLinkIndex(urdf_child_link_name)][0]->getLink_H_geometry();
printToMessageWindow("AFTER link_H_collision_solid_shape " + link_H_visual_solid_shape.toString());

parent_H_child.setPosition(parent_H_child.getPosition() + axis_offset);
*/


// I need to rotate the axis offset by the parent_H_child rotation ????
//axis_offset = parent_H_child.getRotation() * axis_offset;

std::shared_ptr<iDynTree::IJoint> joint_sh_ptr;
if (joint_info.second.type == JointType::Revolute) {
joint_sh_ptr = std::make_shared<iDynTree::RevoluteJoint>();
dynamic_cast<iDynTree::RevoluteJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition() + axis_offset));
dynamic_cast<iDynTree::RevoluteJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition()));
}
else if (joint_info.second.type == JointType::Linear) {
joint_sh_ptr = std::make_shared<iDynTree::PrismaticJoint>();
dynamic_cast<iDynTree::PrismaticJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition() + axis_offset));
dynamic_cast<iDynTree::PrismaticJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition()));
}

joint_sh_ptr->setRestTransform(parent_H_child);
Expand Down
20 changes: 9 additions & 11 deletions src/creo2urdf/src/Utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,22 +172,21 @@ std::pair<bool, iDynTree::Transform> getTransformFromPart(pfcModel_ptr modelhdl,
return { false, H_child };
}

std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array<double, 3>& scale) {
std::tuple<bool, iDynTree::Direction, iDynTree::Transform> getAxisFromPart(pfcModel_ptr modelhdl, const std::string& axis_name, const string& link_frame_name, const array<double, 3>& scale) {

iDynTree::Direction axis_unit_vector;
iDynTree::Position axis_offset;
axis_offset.zero();
iDynTree::Transform oldChild_H_newChild = iDynTree::Transform::Identity();
axis_unit_vector.zero();

auto axes_list = modelhdl->ListItems(pfcModelItemType::pfcITEM_AXIS);
if (axes_list->getarraysize() == 0) {
printToMessageWindow("There is no Axis in the part " + string(modelhdl->GetFullName()), c2uLogLevel::WARN);

return { false, axis_unit_vector, axis_offset };
return { false, axis_unit_vector, oldChild_H_newChild };
}

if (axis_name.empty())
return { false, axis_unit_vector, axis_offset };
return { false, axis_unit_vector, oldChild_H_newChild };

pfcAxis* axis = nullptr;

Expand All @@ -208,11 +207,11 @@ std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcMod
// We use the medium point of the axis as offset
pfcPoint3D_ptr pstart = axis_line->GetEnd1();
pfcPoint3D_ptr pend = axis_line->GetEnd2();
axis_offset[0] = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0];
axis_offset[1] = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1];
axis_offset[2] = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2];
auto x = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0];
auto y = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1];
auto z = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2];
oldChild_H_newChild.setPosition({ x, y, z });

printToMessageWindow("Axis offset: " + axis_offset.toString());
}

// There are just two points in the array
Expand All @@ -227,12 +226,11 @@ std::tuple<bool, iDynTree::Direction, iDynTree::Position> getAxisFromPart(pfcMod


auto& csys_H_child = getTransformFromPart(modelhdl, link_frame_name, scale).second;
printToMessageWindow("csys_H_child: " + csys_H_child.toString());

axis_unit_vector = csys_H_child.inverse() * axis_unit_vector; // We might benefit from performing this operation directly in Creo
axis_unit_vector.Normalize();

return { true, axis_unit_vector, axis_offset };
return { true, axis_unit_vector, oldChild_H_newChild };
}

std::string extractFolderPath(const std::string& filePath) {
Expand Down

0 comments on commit 1650c8f

Please sign in to comment.