Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
pinocchio
Commits
a8f5bc9a
Verified
Commit
a8f5bc9a
authored
Oct 02, 2020
by
Justin Carpentier
Browse files
parsers: read friction and damping parameters
parent
62c7413b
Changes
3
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
parent
I
d
,
JointIndex
parent
_i
d
,
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
[
j
model
.
id
()
]),
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
!
model
.
existJointName
(
modelAB
.
names
[
j
oint_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
=
j
id
;
frame
.
parent
=
j
oint_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
==
j
model
.
id
()
)
if
(
go
.
parentJoint
==
j
oint_id_in
)
{
go
.
parentJoint
=
j
id
;
assert
(
go
.
parentFrame
>
0
);
if
(
go
.
parentFrame
!=
0
)
go
.
parentJoint
=
j
oint_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
id
x
;
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
:
id
x
=
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
:
id
x
=
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
:
id
x
=
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
:
id
x
=
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
(
id
x
,
(
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."
);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment