Skip to content

GitLab

Verified Commit a8f5bc9a authored by Justin Carpentier's avatar Justin Carpentier
Browse files

parsers: read friction and damping parameters

parent 62c7413b
Hide whitespace changes
Inline Side-by-side
src/algorithm/model.hxx
View file @ a8f5bc9a
......@@ -97,44 +97,51 @@ namespace pinocchio
GeometryModel& > ArgsType;
template <typename JointModel>
static void algo (const JointModelBase<JointModel> & jmodel,
static void algo (const JointModelBase<JointModel> & jmodel_in,
const Model & modelAB,
const GeometryModel & geomModelAB,
JointIndex parentId,
JointIndex parent_id,
const typename Model::SE3 & pMi,
Model & model,
GeometryModel & geomModel)
{
// If old parent is universe, use what's provided in the input.
// otherwise, get the parent from modelAB.
if (modelAB.parents[jmodel.id()] > 0)
parentId = model.getJointId(modelAB.names[modelAB.parents[jmodel.id()]]);
const JointIndex joint_id_in = jmodel_in.id();
if (modelAB.parents[joint_id_in] > 0)
parent_id = model.getJointId(modelAB.names[modelAB.parents[joint_id_in]]);
PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existJointName(modelAB.names[jmodel.id()]),
PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existJointName(modelAB.names[joint_id_in]),
"The two models have conflicting joint names.");
JointIndex jid = model.addJoint (
parentId,
jmodel,
pMi * modelAB.jointPlacements[jmodel.id()],
modelAB.names[jmodel.id()],
jmodel.jointVelocitySelector(modelAB.effortLimit),
jmodel.jointVelocitySelector(modelAB.velocityLimit),
jmodel.jointConfigSelector(modelAB.lowerPositionLimit),
jmodel.jointConfigSelector(modelAB.upperPositionLimit));
assert (jid < model.joints.size());
JointIndex joint_id_out = model.addJoint(parent_id,
jmodel_in,
pMi * modelAB.jointPlacements[joint_id_in],
modelAB.names[joint_id_in],
jmodel_in.jointVelocitySelector(modelAB.effortLimit),
jmodel_in.jointVelocitySelector(modelAB.velocityLimit),
jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit),
jmodel_in.jointConfigSelector(modelAB.upperPositionLimit),
jmodel_in.jointVelocitySelector(modelAB.friction),
jmodel_in.jointVelocitySelector(modelAB.damping));
assert(joint_id_out < model.joints.size());
model.appendBodyToJoint (jid, modelAB.inertias[jmodel.id()]);
model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]);
const typename Model::JointModel & jmodel_out = model.joints[joint_id_out];
jmodel_out.jointVelocitySelector(model.rotorInertia) = jmodel_in.jointVelocitySelector(modelAB.rotorInertia);
jmodel_out.jointVelocitySelector(model.rotorGearRatio) = jmodel_in.jointVelocitySelector(modelAB.rotorGearRatio);
// Add all frames whose parent is this joint.
for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) {
for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
{
Frame frame = modelAB.frames[fid];
if (frame.parent == jmodel.id())
if (frame.parent == jmodel_in.id())
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existFrame(frame.name, frame.type),
"The two models have conflicting frame names.");
frame.parent = jid;
frame.parent = joint_id_out;
assert (frame.previousFrame > 0 || frame.type == JOINT);
if (frame.previousFrame != 0)
{
......@@ -146,19 +153,19 @@ namespace pinocchio
}
}
// Add all geometries whose parent is this joint.
for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)
for(GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)
{
GeometryObject go = geomModelAB.geometryObjects[gid];
if (go.parentJoint == jmodel.id())
if(go.parentJoint == joint_id_in)
{
go.parentJoint = jid;
assert (go.parentFrame > 0);
if (go.parentFrame != 0)
go.parentJoint = joint_id_out;
assert(go.parentFrame > 0);
if(go.parentFrame != 0)
{
go.parentFrame = model.getFrameId(modelAB.frames[go.parentFrame].name,
modelAB.frames[go.parentFrame].type);
}
geomModel.addGeometryObject (go);
geomModel.addGeometryObject(go);
}
}
}
......@@ -389,6 +396,17 @@ namespace pinocchio
reduced_model.appendBodyToJoint(reduced_joint_id,
input_model.inertias[joint_id],
SE3::Identity());
// Copy other kinematics and dynamics properties
const typename Model::JointModel & jmodel_out = reduced_model.joints[reduced_joint_id];
jmodel_out.jointVelocitySelector(reduced_model.rotorInertia)
= joint_input_model.jointVelocitySelector(input_model.rotorInertia);
jmodel_out.jointVelocitySelector(reduced_model.rotorGearRatio)
= joint_input_model.jointVelocitySelector(input_model.rotorGearRatio);
jmodel_out.jointVelocitySelector(reduced_model.friction)
= joint_input_model.jointVelocitySelector(input_model.friction);
jmodel_out.jointVelocitySelector(reduced_model.damping)
= joint_input_model.jointVelocitySelector(input_model.damping);
}
}
......
src/parsers/urdf/model.cpp
View file @ a8f5bc9a
......@@ -93,6 +93,7 @@ namespace pinocchio
const Inertia Y = convertFromUrdf(link->inertial);
Vector max_effort(1), max_velocity(1), min_config(1), max_config(1);
Vector friction(Vector::Constant(1,0.)), damping(Vector::Constant(1,0.));
Vector3 axis (joint->axis.x, joint->axis.y, joint->axis.z);
const Scalar infty = std::numeric_limits<Scalar>::infinity();
......@@ -108,11 +109,15 @@ namespace pinocchio
max_config = Vector::Constant(7, infty);
min_config.tail<4>().setConstant(-1.01);
max_config.tail<4>().setConstant( 1.01);
friction = Vector::Constant(6, 0.);
damping = Vector::Constant(6, 0.);
model.addJointAndBody(UrdfVisitorBase::FLOATING, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config,
friction,damping);
break;
case ::urdf::Joint::REVOLUTE:
......@@ -120,18 +125,25 @@ namespace pinocchio
// TODO I think the URDF standard forbids REVOLUTE with no limits.
assert(joint->limits);
if (joint->limits)
if(joint->limits)
{
max_effort << joint->limits->effort;
max_velocity << joint->limits->velocity;
min_config << joint->limits->lower;
max_config << joint->limits->upper;
}
if(joint->dynamics)
{
friction << joint->dynamics->friction;
damping << joint->dynamics->damping;
}
model.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config,
friction,damping);
break;
case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
......@@ -146,15 +158,24 @@ namespace pinocchio
{
max_effort << joint->limits->effort;
max_velocity << joint->limits->velocity;
} else {
}
else
{
max_effort << infty;
max_velocity << infty;
}
if(joint->dynamics)
{
friction << joint->dynamics->friction;
damping << joint->dynamics->damping;
}
model.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config,
friction,damping);
break;
case ::urdf::Joint::PRISMATIC:
......@@ -169,11 +190,18 @@ namespace pinocchio
min_config << joint->limits->lower;
max_config << joint->limits->upper;
}
if(joint->dynamics)
{
friction << joint->dynamics->friction;
damping << joint->dynamics->damping;
}
model.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config,
friction,damping);
break;
case ::urdf::Joint::PLANAR:
......@@ -185,11 +213,15 @@ namespace pinocchio
max_config = Vector::Constant(4, infty);
min_config.tail<2>().setConstant(-1.01);
max_config.tail<2>().setConstant( 1.01);
friction = Vector::Constant(3, 0.);
damping = Vector::Constant(3, 0.);
model.addJointAndBody(UrdfVisitorBase::PLANAR, axis,
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config);
parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,min_config,max_config,
friction,damping);
break;
case ::urdf::Joint::FIXED:
......
src/parsers/urdf/model.hxx
View file @ a8f5bc9a
......@@ -53,7 +53,10 @@ namespace pinocchio
const VectorConstRef& max_effort,
const VectorConstRef& max_velocity,
const VectorConstRef& min_config,
const VectorConstRef& max_config) = 0;
const VectorConstRef& max_config,
const VectorConstRef& friction,
const VectorConstRef& damping
) = 0;
virtual void addFixedJointAndBody(
const FrameIndex & parentFrameId,
......@@ -127,60 +130,67 @@ namespace pinocchio
const VectorConstRef& max_effort,
const VectorConstRef& max_velocity,
const VectorConstRef& min_config,
const VectorConstRef& max_config)
const VectorConstRef& max_config,
const VectorConstRef& friction,
const VectorConstRef& damping)
{
JointIndex idx;
JointIndex joint_id;
const Frame & frame = model.frames[parentFrameId];
switch (type) {
case Base::FLOATING:
idx = model.addJoint(frame.parent,
typename JointCollection::JointModelFreeFlyer(),
frame.placement * placement,
joint_name,
max_effort,max_velocity,min_config,max_config
);
joint_id = model.addJoint(frame.parent,
typename JointCollection::JointModelFreeFlyer(),
frame.placement * placement,
joint_name,
max_effort,max_velocity,min_config,max_config,
friction,damping
);
break;
case Base::REVOLUTE:
idx = addJoint<
joint_id = addJoint<
typename JointCollection::JointModelRX,
typename JointCollection::JointModelRY,
typename JointCollection::JointModelRZ,
typename JointCollection::JointModelRevoluteUnaligned> (
axis, frame, placement, joint_name,
max_effort, max_velocity, min_config, max_config);
max_effort, max_velocity, min_config, max_config,
friction, damping);
break;
case Base::CONTINUOUS:
idx = addJoint<
joint_id = addJoint<
typename JointCollection::JointModelRUBX,
typename JointCollection::JointModelRUBY,
typename JointCollection::JointModelRUBZ,
typename JointCollection::JointModelRevoluteUnboundedUnaligned> (
typename JointCollection::JointModelRevoluteUnboundedUnaligned>(
axis, frame, placement, joint_name,
max_effort, max_velocity, min_config, max_config);
max_effort, max_velocity, min_config, max_config,
friction, damping);
break;
case Base::PRISMATIC:
idx = addJoint<
joint_id = addJoint<
typename JointCollection::JointModelPX,
typename JointCollection::JointModelPY,
typename JointCollection::JointModelPZ,
typename JointCollection::JointModelPrismaticUnaligned> (
axis, frame, placement, joint_name,
max_effort, max_velocity, min_config, max_config);
max_effort, max_velocity, min_config, max_config,
friction, damping);
break;
case Base::PLANAR:
idx = model.addJoint(frame.parent,
joint_id = model.addJoint(frame.parent,
typename JointCollection::JointModelPlanar(),
frame.placement * placement,
joint_name,
max_effort,max_velocity,min_config,max_config
max_effort,max_velocity,min_config,max_config,
friction, damping
);
break;
default:
PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct.");
};
FrameIndex jointFrameId = model.addJointFrame(idx, (int)parentFrameId);
FrameIndex jointFrameId = model.addJointFrame(joint_id, (int)parentFrameId);
appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name);
}
......@@ -270,7 +280,9 @@ namespace pinocchio
const VectorConstRef& max_effort,
const VectorConstRef& max_velocity,
const VectorConstRef& min_config,
const VectorConstRef& max_config)
const VectorConstRef& max_config,
const VectorConstRef& friction,
const VectorConstRef& damping)
{
CartesianAxis axisType = extractCartesianAxis(axis);
switch (axisType)
......@@ -278,25 +290,29 @@ namespace pinocchio
case AXIS_X:
return model.addJoint(frame.parent, TypeX(),
frame.placement * placement, joint_name,
max_effort,max_velocity,min_config,max_config);
max_effort,max_velocity,min_config,max_config,
friction, damping);
break;
case AXIS_Y:
return model.addJoint(frame.parent, TypeY(),
frame.placement * placement, joint_name,
max_effort,max_velocity,min_config,max_config);
max_effort,max_velocity,min_config,max_config,
friction, damping);
break;
case AXIS_Z:
return model.addJoint(frame.parent, TypeZ(),
frame.placement * placement, joint_name,
max_effort,max_velocity,min_config,max_config);
max_effort,max_velocity,min_config,max_config,
friction, damping);
break;
case AXIS_UNALIGNED:
return model.addJoint(frame.parent, TypeUnaligned (axis.normalized()),
frame.placement * placement, joint_name,
max_effort,max_velocity,min_config,max_config);
max_effort,max_velocity,min_config,max_config,
friction, damping);
break;
default:
PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type.");
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment