Unverified Commit 1eedc8eb authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1570 from proyan/topic/modelitem

Introduce ModelItem as parent of Frame and GeometryObject
parents 9dec4b77 6634b3c2
Pipeline #17133 failed with stage
in 136 minutes and 2 seconds
......@@ -8,6 +8,7 @@
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/frame.hpp"
#include "pinocchio/bindings/python/utils/deprecation.hpp"
#include "pinocchio/bindings/python/utils/cast.hpp"
#include "pinocchio/bindings/python/utils/copyable.hpp"
#include "pinocchio/bindings/python/utils/printable.hpp"
......@@ -37,8 +38,16 @@ namespace pinocchio
.def(bp::init<const Frame &>((bp::arg("self"),bp::arg("clone")),"Copy constructor"))
.def_readwrite("name", &Frame::name, "name of the frame")
.def_readwrite("parent", &Frame::parent, "id of the parent joint")
.def_readwrite("previousFrame", &Frame::previousFrame, "id of the previous frame")
.def_readwrite("parentJoint", &Frame::parentJoint, "id of the parent joint")
.def_readwrite("parentFrame", &Frame::parentFrame, "id of the parent frame")
.add_property("parent",
bp::make_getter(&Frame::parentJoint,
deprecated_member<bp::return_value_policy<bp::copy_non_const_reference> >("Deprecated. Use parentJoint")),
bp::make_setter(&Frame::parentJoint, deprecated_member<>("Deprecated. Use parentJoint")), "See parent joint")
.add_property("previousFrame",
bp::make_getter(&Frame::parentFrame,
deprecated_member<bp::return_value_policy<bp::copy_non_const_reference> >("Deprecated. Use parentFrame")),
bp::make_setter(&Frame::parentFrame, deprecated_member<>("Deprecated. Use parentFrame")), "See parent frame")
.def_readwrite("placement",
&Frame::placement,
"placement in the parent joint local frame")
......@@ -89,14 +98,14 @@ namespace pinocchio
static bp::tuple getstate(const Frame & f)
{
return bp::make_tuple(f.name, f.parent, f.previousFrame, f.placement, (int)f.type, f.inertia);
return bp::make_tuple(f.name, f.parentJoint, f.parentFrame, f.placement, (int)f.type, f.inertia);
}
static void setstate(Frame & f, bp::tuple tup)
{
f.name = bp::extract<std::string>(tup[0]);
f.parent = bp::extract<JointIndex>(tup[1]);
f.previousFrame = bp::extract<JointIndex>(tup[2]);
f.parentJoint = bp::extract<JointIndex>(tup[1]);
f.parentFrame = bp::extract<JointIndex>(tup[2]);
f.placement = bp::extract<SE3&>(tup[3]);
f.type = (FrameType)(int)bp::extract<int>(tup[4]);
if(bp::len(tup) > 5)
......
......@@ -9,6 +9,7 @@
#include <eigenpy/memory.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include "pinocchio/bindings/python/utils/registration.hpp"
#include "pinocchio/bindings/python/utils/deprecation.hpp"
#include "pinocchio/multibody/geometry.hpp"
......@@ -30,19 +31,32 @@ namespace pinocchio
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string,JointIndex,FrameIndex,SE3,CollisionGeometryPtr,
bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string> >
(
bp::args("self","name","parent_joint","parent_frame","placement","collision_geometry",
"mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path"),
"Full constructor of a GeometryObject."))
.def(bp::init<std::string,JointIndex,SE3,CollisionGeometryPtr,
bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string> >
(
bp::args("self","name","parent_joint","placement","collision_geometry",
"mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path"),
"Reduced constructor of a GeometryObject. This constructor does not require to specify the parent frame index."
))
.def(bp::init<std::string,FrameIndex,JointIndex,CollisionGeometryPtr,SE3,
bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string> >
(
bp::args("self","name","parent_frame","parent_joint","collision_geometry",
"placement", "mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path"),
"Full constructor of a GeometryObject."))
"Deprecated. Full constructor of a GeometryObject.")[deprecated_function<>()] )
.def(bp::init<std::string,JointIndex,CollisionGeometryPtr,SE3,
bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string> >
(
bp::args("self","name","parent_joint","collision_geometry",
"placement", "mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path"),
"Reduced constructor of a GeometryObject. This constructor does not require to specify the parent frame index."
))
"Deprecated. Reduced constructor of a GeometryObject. This constructor does not require to specify the parent frame index."
)[deprecated_function<>()] )
.def(bp::init<const GeometryObject&>
(
bp::args("self","otherGeometryObject"),
......@@ -87,9 +101,9 @@ namespace pinocchio
#ifdef PINOCCHIO_WITH_HPP_FCL
static GeometryObject maker_capsule(const double radius, const double length)
{
return GeometryObject("",FrameIndex(0),JointIndex(0),
boost::shared_ptr<fcl::CollisionGeometry>(new fcl::Capsule(radius, length)),
SE3::Identity());
return GeometryObject("",JointIndex(0),FrameIndex(0),
SE3::Identity(),
boost::shared_ptr<fcl::CollisionGeometry>(new fcl::Capsule(radius, length)));
}
#endif // PINOCCHIO_WITH_HPP_FCL
......
......@@ -20,7 +20,7 @@ geometries = [
]
for i, geom in enumerate(geometries):
placement = pin.SE3(np.eye(3), np.array([i, 0, 0]))
geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, geom, placement)
geom_obj = pin.GeometryObject("obj{}".format(i), 0, 0, placement,geom)
color = np.random.uniform(0, 1, 4)
color[3] = 1
geom_obj.meshColor = color
......
......@@ -15,7 +15,7 @@ body_mass = 1.
body_radius = 0.1
shape0 = fcl.Sphere(body_radius)
geom0_obj = pin.GeometryObject("base", 0, shape0, pin.SE3.Identity())
geom0_obj = pin.GeometryObject("base", 0, pin.SE3.Identity(), shape0)
geom0_obj.meshColor = np.array([1.,0.1,0.1,1.])
geom_model.addGeometryObject(geom0_obj)
......@@ -30,7 +30,7 @@ for k in range(N):
geom1_name = "ball_" + str(k+1)
shape1 = fcl.Sphere(body_radius)
geom1_obj = pin.GeometryObject(geom1_name, joint_id, shape1, body_placement)
geom1_obj = pin.GeometryObject(geom1_name, joint_id, body_placement, shape1)
geom1_obj.meshColor = np.ones((4))
geom_model.addGeometryObject(geom1_obj)
......@@ -39,7 +39,7 @@ for k in range(N):
shape2_placement = body_placement.copy()
shape2_placement.translation[2] /= 2.
geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2, shape2_placement)
geom2_obj = pin.GeometryObject(geom2_name, joint_id, shape2_placement, shape2)
geom2_obj.meshColor = np.array([0.,0.,0.,1.])
geom_model.addGeometryObject(geom2_obj)
......
......@@ -70,8 +70,8 @@ namespace pinocchio
const Frame & frame = model.frames[frame_id];
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[frame.parent] * frame.placement; // for backward compatibility
getFrameVelocityDerivatives(model,data,frame.parent,frame.placement,rf,
oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
getFrameVelocityDerivatives(model,data,frame.parentJoint,frame.placement,rf,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dv));
}
......@@ -149,8 +149,8 @@ namespace pinocchio
const Frame & frame = model.frames[frame_id];
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[frame.parent] * frame.placement; // for backward compatibility
getFrameAccelerationDerivatives(model,data,frame.parent,frame.placement,rf,
oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
getFrameAccelerationDerivatives(model,data,frame.parentJoint,frame.placement,rf,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,a_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dv),
......@@ -246,8 +246,8 @@ namespace pinocchio
const Frame & frame = model.frames[frame_id];
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[frame.parent] * frame.placement; // for backward compatibility
getFrameAccelerationDerivatives(model,data,frame.parent,frame.placement,rf,
oMframe = data.oMi[frame.parentJoint] * frame.placement; // for backward compatibility
getFrameAccelerationDerivatives(model,data,frame.parentJoint,frame.placement,rf,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dq),
......
......@@ -129,7 +129,7 @@ namespace pinocchio
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
const typename Model::Frame & frame = model.frames[frame_id];
return getFrameVelocity(model, data, frame.parent, frame.placement, rf);
return getFrameVelocity(model, data, frame.parentJoint, frame.placement, rf);
}
......@@ -180,7 +180,7 @@ namespace pinocchio
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
const typename Model::Frame & frame = model.frames[frame_id];
return getFrameAcceleration(model, data, frame.parent, frame.placement, rf);
return getFrameAcceleration(model, data, frame.parentJoint, frame.placement, rf);
}
......@@ -234,7 +234,7 @@ namespace pinocchio
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
const typename Model::Frame & frame = model.frames[frame_id];
return getFrameClassicalAcceleration(model, data, frame.parent, frame.placement, rf);
return getFrameClassicalAcceleration(model, data, frame.parentJoint, frame.placement, rf);
}
......@@ -338,9 +338,9 @@ namespace pinocchio
typedef typename Model::Frame Frame;
const Frame & frame = model.frames[frame_id];
data.oMf[frame_id] = data.oMi[frame.parent] * frame.placement;
data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement;
getFrameJacobian(model,data,frame.parent,frame.placement,reference_frame,
getFrameJacobian(model,data,frame.parentJoint,frame.placement,reference_frame,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
}
......
......@@ -28,7 +28,7 @@ namespace pinocchio
for(FrameIndex i=1; i < (FrameIndex) model.nframes; ++i)
{
const Frame & frame = model.frames[i];
const JointIndex & parent = frame.parent;
const JointIndex & parent = frame.parentJoint;
data.oMf[i] = data.oMi[parent]*frame.placement;
}
}
......@@ -43,7 +43,7 @@ namespace pinocchio
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
const typename Model::Frame & frame = model.frames[frame_id];
const typename Model::JointIndex & parent = frame.parent;
const typename Model::JointIndex & parent = frame.parentJoint;
data.oMf[frame_id] = data.oMi[parent]*frame.placement;
......@@ -178,7 +178,7 @@ namespace pinocchio
typedef typename Model::IndexVector IndexVector;
const Frame & frame = model.frames[frameId];
const JointIndex & joint_id = frame.parent;
const JointIndex & joint_id = frame.parentJoint;
const IndexVector & joint_support = model.supports[joint_id];
......@@ -249,7 +249,7 @@ namespace pinocchio
typedef typename Model::Frame Frame;
const Frame & frame = model.frames[frame_id];
const JointIndex & joint_id = frame.parent;
const JointIndex & joint_id = frame.parentJoint;
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[joint_id] * frame.placement;
......
......@@ -23,7 +23,7 @@ namespace pinocchio
typedef typename Model::Frame Frame;
const Frame & pframe = model.frames[parentFrame];
JointIndex jid = pframe.parent;
JointIndex jid = pframe.parentJoint;
assert(jid < model.joints.size());
// If inertia is not NaN, add it.
......@@ -34,21 +34,21 @@ namespace pinocchio
for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
{
Frame frame = modelAB.frames[fid];
if (frame.parent == 0)
if (frame.parentJoint == 0)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existFrame(frame.name, frame.type),
"The two models have conflicting frame names.");
frame.parent = jid;
if (frame.previousFrame != 0)
frame.parentJoint = jid;
if (frame.parentFrame != 0)
{
frame.previousFrame = model.getFrameId (
modelAB.frames[frame.previousFrame].name,
modelAB.frames[frame.previousFrame].type);
frame.parentFrame = model.getFrameId (
modelAB.frames[frame.parentFrame].name,
modelAB.frames[frame.parentFrame].type);
}
else
{
frame.previousFrame = parentFrame;
frame.parentFrame = parentFrame;
}
// Modify frame placement
......@@ -136,17 +136,17 @@ namespace pinocchio
for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
{
Frame frame = modelAB.frames[fid];
if (frame.parent == jmodel_in.id())
if (frame.parentJoint == jmodel_in.id())
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existFrame(frame.name, frame.type),
"The two models have conflicting frame names.");
frame.parent = joint_id_out;
assert (frame.previousFrame > 0 || frame.type == JOINT);
if (frame.previousFrame != 0)
frame.parentJoint = joint_id_out;
assert (frame.parentFrame > 0 || frame.type == JOINT);
if (frame.parentFrame != 0)
{
frame.previousFrame = model.getFrameId(modelAB.frames[frame.previousFrame].name,
modelAB.frames[frame.previousFrame].type);
frame.parentFrame = model.getFrameId(modelAB.frames[frame.parentFrame].name,
modelAB.frames[frame.parentFrame].type);
}
model.addFrame(frame);
......@@ -223,7 +223,7 @@ namespace pinocchio
// Copy modelA joints until frame.parentJoint
details::appendUniverseToModel (modelA, geomModelA, 0, id, model, geomModel);
for (JointIndex jid = 1; jid <= frame.parent; ++jid)
for (JointIndex jid = 1; jid <= frame.parentJoint; ++jid)
{
ArgsType args (modelA, geomModelA, 0, id, model, geomModel);
AppendJointOfModelAlgo::run (modelA.joints[jid], args);
......@@ -235,12 +235,12 @@ namespace pinocchio
for (JointIndex jid = 1; jid < modelB.joints.size(); ++jid)
{
SE3 pMi = (jid == 1 ? frame.placement * aMb : id);
ArgsType args (modelB, geomModelB, frame.parent, pMi, model, geomModel);
ArgsType args (modelB, geomModelB, frame.parentJoint, pMi, model, geomModel);
AppendJointOfModelAlgo::run (modelB.joints[jid], args);
}
// Copy remaining joints of modelA
for (JointIndex jid = frame.parent+1; jid < modelA.joints.size(); ++jid)
for (JointIndex jid = frame.parentJoint+1; jid < modelA.joints.size(); ++jid)
{
ArgsType args (modelA, geomModelA, 0, id, model, geomModel);
AppendJointOfModelAlgo::run (modelA.joints[jid], args);
......@@ -345,7 +345,7 @@ namespace pinocchio
const JointIndex reduced_parent_joint_index
= exist_parent_joint
? reduced_model.getJointId(parent_joint_name)
: reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].parent;
: reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].parentJoint;
const SE3 parent_frame_placement
= exist_parent_joint
......@@ -373,7 +373,7 @@ namespace pinocchio
input_model.inertias[joint_id]);
FrameIndex frame_id = reduced_model.addFrame(frame);
reduced_model.frames[frame_id].previousFrame = frame_id; // a bit weird, but this is a solution for missing parent frame
reduced_model.frames[frame_id].parentFrame = frame_id; // a bit weird, but this is a solution for missing parent frame
current_index_to_lock++;
}
......@@ -432,11 +432,11 @@ namespace pinocchio
for(++frame_it;frame_it != input_model.frames.end(); ++frame_it)
{
const Frame & input_frame = *frame_it;
const std::string & support_joint_name = input_model.names[input_frame.parent];
const std::string & support_joint_name = input_model.names[input_frame.parentJoint];
std::vector<JointIndex>::const_iterator support_joint_it = std::find(list_of_joints_to_lock.begin(),
list_of_joints_to_lock.end(),
input_frame.parent);
input_frame.parentJoint);
if(support_joint_it != list_of_joints_to_lock.end())
{
......@@ -450,15 +450,15 @@ namespace pinocchio
const Frame & joint_frame = reduced_model.frames[joint_frame_id];
Frame reduced_frame = input_frame;
reduced_frame.placement = joint_frame.placement * input_frame.placement;
reduced_frame.parent = joint_frame.parent;
reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
reduced_frame.parentJoint = joint_frame.parentJoint;
reduced_frame.parentFrame = reduced_model.getFrameId(input_model.frames[input_frame.parentFrame].name);
reduced_model.addFrame(reduced_frame, false);
}
else
{
Frame reduced_frame = input_frame;
reduced_frame.parent = reduced_model.getJointId(input_model.names[input_frame.parent]);
reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
reduced_frame.parentJoint = reduced_model.getJointId(input_model.names[input_frame.parentJoint]);
reduced_frame.parentFrame = reduced_model.getFrameId(input_model.frames[input_frame.parentFrame].name);
reduced_model.addFrame(reduced_frame, false);
}
}
......@@ -526,7 +526,7 @@ namespace pinocchio
else // The joint is now a frame
{
const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
reduced_joint_id = reduced_model.frames[reduced_frame_id].parent;
reduced_joint_id = reduced_model.frames[reduced_frame_id].parentJoint;
relative_placement = reduced_model.frames[reduced_frame_id].placement;
}
......
......@@ -104,9 +104,9 @@ namespace pinocchio
typedef typename Model::Frame Frame;
const Frame & frame = model.frames[frame_id];
data.oMf[frame_id] = data.oMi[frame.parent] * frame.placement;
data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement;
internal::computeJointKinematicRegressorGeneric(model,data,frame.parent,rf,data.oMf[frame_id],
internal::computeJointKinematicRegressorGeneric(model,data,frame.parentJoint,rf,data.oMf[frame_id],
kinematic_regressor.const_cast_derived());
}
......@@ -265,7 +265,7 @@ namespace pinocchio
typedef typename Model::SE3 SE3;
const Frame & frame = model.frames[frameId];
const JointIndex & parent = frame.parent;
const JointIndex & parent = frame.parentJoint;
const SE3 & placement = frame.placement;
bodyRegressor(placement.actInv(data.v[parent]), placement.actInv(data.a_gf[parent]), data.bodyRegressor);
......
......@@ -7,6 +7,7 @@
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/model-item.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#ifdef PINOCCHIO_WITH_HPP_FCL
......@@ -40,7 +41,6 @@
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
#endif
#include <iostream>
#include <map>
#include <vector>
#include <utility>
......@@ -58,7 +58,7 @@ namespace pinocchio
typedef std::pair<GeomIndex, GeomIndex> Base;
/// \brief Empty constructor
/// \brief Empty constructor
CollisionPair();
///
......@@ -115,24 +115,31 @@ enum GeometryType
COLLISION
};
struct GeometryObject
struct GeometryObject; //fwd
template<>
struct traits<GeometryObject>
{
typedef double Scalar;
enum { Options = 0};
};
struct GeometryObject : public ModelItem<GeometryObject>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef ModelItem<GeometryObject> Base;
typedef typename traits<GeometryObject>::Scalar Scalar;
enum { Options = traits<GeometryObject>::Options };
typedef SE3Tpl<Scalar,Options> SE3;
typedef boost::shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr;
/// \brief Name of the geometry object
std::string name;
/// \brief Index of the parent frame
///
/// Parent frame may be unset (to the std::numeric_limits<FrameIndex>::max() value) as it is mostly used as a documentation of the tree, or in third-party libraries.
/// The URDF parser of Pinocchio is setting it to the proper value according to the urdf link-joint tree.
/// In particular, anchor joints of URDF would cause parent frame to be different to joint frame.
FrameIndex parentFrame;
/// \brief Index of the parent joint
JointIndex parentJoint;
using Base::name;
using Base::parentFrame;
using Base::parentJoint;
using Base::placement;
/// \brief The FCL CollisionGeometry (might be a Mesh, a Geometry Primitive, etc.)
CollisionGeometryPtr geometry;
......@@ -141,9 +148,6 @@ struct GeometryObject
/// \deprecated It is now deprecated and has been renamed GeometryObject::geometry
PINOCCHIO_DEPRECATED CollisionGeometryPtr & fcl;
/// \brief Position of geometry object in parent joint frame
SE3 placement;
/// \brief Absolute path to the mesh file (if the fcl pointee is also a Mesh)
std::string meshPath;
......@@ -168,10 +172,10 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
/// \brief Full constructor.
///
/// \param[in] name Name of the geometry object.
/// \param[in] parent_frame Index of the parent frame.
/// \param[in] parent_joint Index of the parent joint (that supports the geometry).
/// \param[in] collision_geometry The FCL collision geometry object.
/// \param[in] parent_frame Index of the parent frame.
/// \param[in] placement Placement of the geometry with respect to the joint frame.
/// \param[in] collision_geometry The FCL collision geometry object.
/// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a viewer for instance) [if applicable].
/// \param[in] meshScale Scale of the mesh [if applicable].
/// \param[in] overrideMaterial If true, this option allows to overrite the material [if applicable].
......@@ -179,21 +183,18 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
/// \param[in] meshTexturePath Path to the file containing the texture information [if applicable].
///
GeometryObject(const std::string & name,
const FrameIndex parent_frame,
const JointIndex parent_joint,
const CollisionGeometryPtr & collision_geometry,
const FrameIndex parent_frame,
const SE3 & placement,
const CollisionGeometryPtr & collision_geometry,
const std::string & meshPath = "",
const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
const bool overrideMaterial = false,
const Eigen::Vector4d & meshColor = Eigen::Vector4d::Zero(),
const std::string & meshTexturePath = "")
: name(name)
, parentFrame(parent_frame)
, parentJoint(parent_joint)
: Base(name, parent_joint, parent_frame, placement)
, geometry(collision_geometry)
, fcl(geometry)
, placement(placement)
, meshPath(meshPath)
, meshScale(meshScale)
, overrideMaterial(overrideMaterial)
......@@ -201,6 +202,44 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
, meshTexturePath(meshTexturePath)
, disableCollision(false)
{}
///
/// \brief Full constructor.
///
/// \param[in] name Name of the geometry object.
/// \param[in] parent_frame Index of the parent frame.
/// \param[in] parent_joint Index of the parent joint (that supports the geometry).
/// \param[in] collision_geometry The FCL collision geometry object.
/// \param[in] placement Placement of the geometry with respect to the joint frame.
/// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a viewer for instance) [if applicable].
/// \param[in] meshScale Scale of the mesh [if applicable].
/// \param[in] overrideMaterial If true, this option allows to overrite the material [if applicable].
/// \param[in] meshColor Color of the mesh [if applicable].
/// \param[in] meshTexturePath Path to the file containing the texture information [if applicable].
///
/// \deprecated This constructor is now deprecated, and its argument order has been changed.
///
PINOCCHIO_DEPRECATED GeometryObject(const std::string & name,
const FrameIndex parent_frame,
const JointIndex parent_joint,
const CollisionGeometryPtr & collision_geometry,
const SE3 & placement,
const std::string & meshPath = "",
const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
const bool overrideMaterial = false,
const Eigen::Vector4d & meshColor = Eigen::Vector4d::Zero(),
const std::string & meshTexturePath = "")
: Base(name, parent_joint, parent_frame, placement)
, geometry(collision_geometry)
, fcl(geometry)
, meshPath(meshPath)
, meshScale(meshScale)
, overrideMaterial(overrideMaterial)
, meshColor(meshColor)
, meshTexturePath(meshTexturePath)
, disableCollision(false)
{}
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
......@@ -211,8 +250,8 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
///
/// \param[in] name Name of the geometry object.
/// \param[in] parent_joint Index of the parent joint (that supports the geometry).
/// \param[in] collision_geometry The FCL collision geometry object.
/// \param[in] placement Placement of the geometry with respect to the joint frame.
/// \param[in] collision_geometry The FCL collision geometry object.
/// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a viewer for instance) [if applicable].
/// \param[in] meshScale Scale of the mesh [if applicable].
/// \param[in] overrideMaterial If true, this option allows to overrite the material [if applicable].
......@@ -221,19 +260,53 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
///
GeometryObject(const std::string & name,
const JointIndex parent_joint,
const CollisionGeometryPtr & collision_geometry,
const SE3 & placement,
const CollisionGeometryPtr & collision_geometry,
const std::string & meshPath = "",
const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
const bool overrideMaterial = false,
const Eigen::Vector4d & meshColor = Eigen::Vector4d::Zero(),
const std::string & meshTexturePath = "")