model-graph.hpp

//
// Copyright (c) 2025 INRIA
//

#ifndef __pinocchio_parsers_graph_model_graph_hpp__
#define __pinocchio_parsers_graph_model_graph_hpp__

#include "pinocchio/parsers/graph/fwd.hpp"

#include "pinocchio/parsers/config.hpp"

#include "pinocchio/parsers/graph/joints.hpp"
#include "pinocchio/parsers/graph/frames.hpp"
#include "pinocchio/parsers/graph/geometries.hpp"

#include <Eigen/Core>

#include <boost/optional.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <vector>
#include <string>
#include <unordered_map>
#include <stdexcept>

namespace pinocchio
{
 namespace graph
 {
 struct EdgeBuilder;
 struct EdgeParameters;
 struct GeometryBuilder;

 struct ModelGraphVertex
 {
 std::string name;

 FrameVariant frame;

 std::vector<Geometry> geometries;

 void addGeometry(const Geometry & geo)
 {
 geometries.push_back(geo);
 }
 };

 struct ModelGraphEdge
 {
 std::string name;

 JointVariant joint;

 JointLimits jlimit;

 SE3 source_to_joint;

 SE3 joint_to_target;

 bool forward = true;
 };

 struct ModelGraphBuildInfo
 {
 std::unordered_map<std::string, bool> _joint_forward;
 bool _is_fixed;
 };

 struct PINOCCHIO_PARSERS_DLLAPI ModelGraph
 {
 typedef boost::
 adjacency_list<boost::vecS, boost::vecS, boost::directedS, ModelGraphVertex, ModelGraphEdge>
 Graph;
 typedef typename boost::graph_traits<Graph>::vertex_descriptor VertexDesc;
 typedef typename boost::graph_traits<Graph>::edge_descriptor EdgeDesc;

 ModelGraph() = default;
 void addFrame(const std::string & vertex_name, const FrameVariant & frame);

 void addBody(const std::string & vertex_name, const Inertia & inert);

 GeometryBuilder geometryBuilder();
 void addGeometry(const std::string & vertex_name, const Geometry & geom);
 void addGeometries(const std::string & vertex_name, const std::vector<Geometry> & geoms);

 void addJoint(
 const std::string & joint_name,
 const JointVariant & joint,
 const std::string & source_body,
 const SE3 & source_to_joint,
 const std::string & target_body,
 const SE3 & joint_to_target);

 void addJoint(const EdgeParameters & params);

 EdgeBuilder edgeBuilder();

 void appendGraph(const ModelGraph & g);

 Graph graph;
 std::unordered_map<std::string, VertexDesc> name_to_vertex;
 };

 struct PINOCCHIO_PARSERS_DLLAPI EdgeParameters
 {
 std::string name;

 std::string source_vertex;
 SE3 source_to_joint = SE3::Identity();
 std::string target_vertex;
 SE3 joint_to_target = SE3::Identity();

 JointVariant joint = JointFixed();

 boost::optional<Eigen::VectorXd> q_ref = boost::none;

 JointLimits jlimit;

 EdgeParameters() = default;

 EdgeParameters(
 const std::string & jname,
 const std::string & source_name,
 const SE3 & source_to_joint,
 const std::string & target_name,
 const SE3 & joint_to_target,
 const JointVariant & joint,
 const boost::optional<Eigen::VectorXd> q_ref = boost::none);
 };

 struct PINOCCHIO_PARSERS_DLLAPI EdgeBuilder
 {
 ModelGraph & g;

 EdgeParameters param;

 boost::optional<Eigen::VectorXd> minConfig;
 boost::optional<Eigen::VectorXd> maxConfig;
 boost::optional<Eigen::VectorXd> maxVel;
 boost::optional<Eigen::VectorXd> maxEffort;
 boost::optional<Eigen::VectorXd> armature;
 boost::optional<Eigen::VectorXd> friction;
 boost::optional<Eigen::VectorXd> damping;

 double frictionLoss = 0;

 explicit EdgeBuilder(ModelGraph & graph)
 : g(graph)
 {
 }

 EdgeBuilder & withJointType(const JointVariant & jtype)
 {
 param.joint = jtype;
 return *this;
 }

 EdgeBuilder & withName(const std::string & name)
 {
 param.name = name;
 return *this;
 }

 EdgeBuilder & withTargetVertex(const std::string & target_name)
 {
 param.target_vertex = target_name;
 return *this;
 }

 EdgeBuilder & withSourceVertex(const std::string & source_name)
 {
 param.source_vertex = source_name;
 return *this;
 }

 EdgeBuilder & withTargetPose(const SE3 & target_pose)
 {
 param.joint_to_target = target_pose;
 return *this;
 }

 EdgeBuilder & withSourcePose(const SE3 & source_pose)
 {
 param.source_to_joint = source_pose;
 return *this;
 }

 EdgeBuilder & withQref(const Eigen::VectorXd & qref)
 {
 param.q_ref = qref;
 return *this;
 }

 EdgeBuilder & withMinConfig(const Eigen::VectorXd & minConfig_)
 {
 minConfig = minConfig_;
 return *this;
 }

 EdgeBuilder & withMaxConfig(const Eigen::VectorXd & maxConfig_)
 {
 maxConfig = maxConfig_;
 return *this;
 }

 EdgeBuilder & withMaxVel(const Eigen::VectorXd & maxVel_)
 {
 maxVel = maxVel_;
 return *this;
 }

 EdgeBuilder & withMaxEffort(const Eigen::VectorXd & maxEffort_)
 {
 maxEffort = maxEffort_;
 return *this;
 }

 EdgeBuilder & withFriction(const Eigen::VectorXd & friction_)
 {
 friction = friction_;
 return *this;
 }

 EdgeBuilder & withDamping(const Eigen::VectorXd & damping_)
 {
 damping = damping_;
 return *this;
 }

 EdgeBuilder & withArmature(const Eigen::VectorXd & armature_)
 {
 armature = armature_;
 return *this;
 }

 EdgeBuilder & withFrictionLoss(const double frictionLoss_)
 {
 frictionLoss = frictionLoss_;
 return *this;
 }

 void build();
 };

 struct GeometryBuilder
 {
 Geometry geometry;
 std::string name_body;

 ModelGraph & g;

 GeometryBuilder(ModelGraph & g)
 : g(g)
 {
 }

 GeometryBuilder & withName(const std::string & n)
 {
 geometry.name = n;
 return *this;
 }

 GeometryBuilder & withBody(const std::string & n)
 {
 name_body = n;
 return *this;
 }

 GeometryBuilder & withPlacement(const SE3 & p)
 {
 geometry.placement = p;
 return *this;
 }

 GeometryBuilder & withScale(const Eigen::Vector3d & s)
 {
 geometry.scale = s;
 return *this;
 }

 GeometryBuilder & withColor(const Eigen::Vector4d & c)
 {
 geometry.color = c;
 return *this;
 }

 GeometryBuilder & withGeomType(const GeomType type)
 {
 geometry.type = type;
 return *this;
 }

 GeometryBuilder & withGeom(const GeomVariant & g)
 {
 geometry.geometry = g;
 return *this;
 }

 void build()
 {
 if (geometry.name.empty())
 PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Graph - geometry should have a name.");

 return g.addGeometry(name_body, geometry);
 }
 };
 } // namespace graph
} // namespace pinocchio

#endif // ifndef __pinocchio_parsers_graph_model_graph_hpp__