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
aa39638f
Unverified
Commit
aa39638f
authored
Oct 03, 2020
by
Justin Carpentier
Committed by
GitHub
Oct 03, 2020
Browse files
Merge pull request #1304 from jcarpent/topic/devel
Add support of joint friction and dampings
parents
460162f6
82d7e083
Pipeline
#11544
passed with stage
in 161 minutes and 52 seconds
Changes
10
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
bindings/python/multibody/joint/expose-joints.cpp
View file @
aa39638f
...
...
@@ -16,11 +16,11 @@ namespace pinocchio
static
void
exposeVariants
()
{
boost
::
mpl
::
for_each
<
JointModelVariant
::
types
>
(
ModelExposer
());
boost
::
mpl
::
for_each
<
JointModelVariant
::
types
>
(
Joint
ModelExposer
());
bp
::
to_python_converter
<
pinocchio
::
JointModelVariant
,
JointVariantVisitor
<
pinocchio
::
JointModelVariant
>
>
();
boost
::
mpl
::
for_each
<
JointDataVariant
::
types
>
(
DataExposer
());
boost
::
mpl
::
for_each
<
JointDataVariant
::
types
>
(
Joint
DataExposer
());
bp
::
to_python_converter
<
pinocchio
::
JointDataVariant
,
JointVariantVisitor
<
pinocchio
::
JointDataVariant
>
>
();
}
...
...
bindings/python/multibody/joint/joint-derived.hpp
View file @
aa39638f
//
// Copyright (c) 2015
CNRS
// Copyright (c) 2015
-2020 CNRS INRIA
//
#ifndef __pinocchio_python_joint_
dense
_hpp__
#define __pinocchio_python_joint_
dense
_hpp__
#ifndef __pinocchio_python_joint_
variants
_hpp__
#define __pinocchio_python_joint_
variants
_hpp__
#include
<boost/python.hpp>
#include
<eigenpy/exception.hpp>
...
...
@@ -27,27 +27,29 @@ namespace pinocchio
{
cl
// All are add_properties cause ReadOnly
.
add_property
(
"id"
,
&
JointModelDerivedPythonVisitor
::
getI
d
)
.
add_property
(
"idx_q"
,
&
JointModelDerivedPythonVisitor
::
getI
dx_q
)
.
add_property
(
"idx_v"
,
&
JointModelDerivedPythonVisitor
::
getI
dx_v
)
.
add_property
(
"nq"
,
&
JointModelDerivedPythonVisitor
::
getN
q
)
.
add_property
(
"nv"
,
&
JointModelDerivedPythonVisitor
::
getN
v
)
.
add_property
(
"id"
,
&
get_i
d
)
.
add_property
(
"idx_q"
,
&
get_i
dx_q
)
.
add_property
(
"idx_v"
,
&
get_i
dx_v
)
.
add_property
(
"nq"
,
&
get_n
q
)
.
add_property
(
"nv"
,
&
get_n
v
)
.
def
(
"setIndexes"
,
&
JointModelDerived
::
setIndexes
)
.
def
(
"shortname"
,
&
JointModelDerived
::
shortname
)
;
}
static
JointIndex
getId
(
const
JointModelDerived
&
self
)
{
return
self
.
id
();
}
static
int
getIdx_q
(
const
JointModelDerived
&
self
)
{
return
self
.
idx_q
();}
static
int
getIdx_v
(
const
JointModelDerived
&
self
)
{
return
self
.
idx_v
();}
static
int
getNq
(
const
JointModelDerived
&
self
)
{
return
self
.
nq
();}
static
int
getNv
(
const
JointModelDerived
&
self
)
{
return
self
.
nv
();}
static
JointIndex
get_id
(
const
JointModelDerived
&
self
)
{
return
self
.
id
();
}
static
int
get_idx_q
(
const
JointModelDerived
&
self
)
{
return
self
.
idx_q
();
}
static
int
get_idx_v
(
const
JointModelDerived
&
self
)
{
return
self
.
idx_v
();
}
static
int
get_nq
(
const
JointModelDerived
&
self
)
{
return
self
.
nq
();
}
static
int
get_nv
(
const
JointModelDerived
&
self
)
{
return
self
.
nv
();
}
static
void
expose
()
{
}
{}
};
...
...
@@ -62,42 +64,37 @@ namespace pinocchio
{
cl
// All are add_properties cause ReadOnly
.
add_property
(
"S"
,
&
JointDataDerivedPythonVisitor
::
getS
)
.
add_property
(
"M"
,
&
JointDataDerivedPythonVisitor
::
getM
)
.
add_property
(
"v"
,
&
JointDataDerivedPythonVisitor
::
getv
)
.
add_property
(
"c"
,
&
JointDataDerivedPythonVisitor
::
getc
)
.
add_property
(
"U"
,
&
JointDataDerivedPythonVisitor
::
getU
)
.
add_property
(
"Dinv"
,
&
JointDataDerivedPythonVisitor
::
getDinv
)
.
add_property
(
"UDinv"
,
&
JointDataDerivedPythonVisitor
::
getUDinv
)
.
add_property
(
"S"
,
&
get
_
S
)
.
add_property
(
"M"
,
&
get
_
M
)
.
add_property
(
"v"
,
&
get
_
v
)
.
add_property
(
"c"
,
&
get
_
c
)
.
add_property
(
"U"
,
&
get
_
U
)
.
add_property
(
"Dinv"
,
&
get
_
Dinv
)
.
add_property
(
"UDinv"
,
&
get
_
UDinv
)
.
def
(
"shortname"
,
&
JointDataDerived
::
shortname
)
;
}
static
typename
JointDataDerived
::
Constraint_t
getS
(
const
JointDataDerived
&
self
)
static
typename
JointDataDerived
::
Constraint_t
get
_
S
(
const
JointDataDerived
&
self
)
{
return
self
.
S_accessor
();
}
static
typename
JointDataDerived
::
Transformation_t
getM
(
const
JointDataDerived
&
self
)
static
typename
JointDataDerived
::
Transformation_t
get
_
M
(
const
JointDataDerived
&
self
)
{
return
self
.
M_accessor
();
}
static
typename
JointDataDerived
::
Motion_t
getv
(
const
JointDataDerived
&
self
)
static
typename
JointDataDerived
::
Motion_t
get
_
v
(
const
JointDataDerived
&
self
)
{
return
self
.
v_accessor
();
}
static
typename
JointDataDerived
::
Bias_t
getc
(
const
JointDataDerived
&
self
)
static
typename
JointDataDerived
::
Bias_t
get
_
c
(
const
JointDataDerived
&
self
)
{
return
self
.
c_accessor
();
}
static
typename
JointDataDerived
::
U_t
getU
(
const
JointDataDerived
&
self
)
static
typename
JointDataDerived
::
U_t
get
_
U
(
const
JointDataDerived
&
self
)
{
return
self
.
U_accessor
();
}
static
typename
JointDataDerived
::
D_t
getDinv
(
const
JointDataDerived
&
self
)
static
typename
JointDataDerived
::
D_t
get
_
Dinv
(
const
JointDataDerived
&
self
)
{
return
self
.
Dinv_accessor
();
}
static
typename
JointDataDerived
::
UD_t
getUDinv
(
const
JointDataDerived
&
self
)
static
typename
JointDataDerived
::
UD_t
get
_
UDinv
(
const
JointDataDerived
&
self
)
{
return
self
.
UDinv_accessor
();
}
static
void
expose
()
{
}
{}
};
}}
// namespace pinocchio::python
#endif // ifndef __pinocchio_python_joint_dense_hpp__
#endif // ifndef __pinocchio_python_joint_variants_hpp__
bindings/python/multibody/joint/joints-variant.hpp
View file @
aa39638f
...
...
@@ -33,7 +33,7 @@ namespace pinocchio
}
};
struct
DataExposer
struct
Joint
DataExposer
{
template
<
class
T
>
void
operator
()(
T
)
...
...
@@ -49,7 +49,7 @@ namespace pinocchio
}
};
struct
ModelExposer
struct
Joint
ModelExposer
{
template
<
class
T
>
void
operator
()(
T
)
...
...
bindings/python/multibody/model.hpp
View file @
aa39638f
...
...
@@ -97,7 +97,7 @@ namespace pinocchio
protected:
struct
addJointVisitor
struct
addJointVisitor
0
:
public
boost
::
static_visitor
<
Index
>
{
Model
&
m_model
;
...
...
@@ -105,10 +105,10 @@ namespace pinocchio
const
SE3
&
m_joint_placement
;
const
std
::
string
&
m_joint_name
;
addJointVisitor
(
Model
&
model
,
const
JointIndex
parent_id
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
)
addJointVisitor
0
(
Model
&
model
,
const
JointIndex
parent_id
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
)
:
m_model
(
model
)
,
m_parent_id
(
parent_id
)
,
m_joint_placement
(
joint_placement
)
...
...
@@ -120,21 +120,22 @@ namespace pinocchio
{
return
m_model
.
addJoint
(
m_parent_id
,
jmodel
,
m_joint_placement
,
m_joint_name
);
}
};
// struct addJointVisitor
};
// struct addJointVisitor
0
struct
addJoint
WithLimits
Visitor
:
public
boost
::
static_visitor
<
Index
>
struct
addJointVisitor
1
:
public
addJointVisitor0
{
Model
&
m_model
;
const
JointIndex
m_parent_id
;
const
SE3
&
m_joint_placement
;
const
std
::
string
&
m_joint_name
;
using
addJointVisitor0
::
m_model
;
using
addJointVisitor0
::
m_parent_id
;
using
addJointVisitor0
::
m_joint_placement
;
using
addJointVisitor0
::
m_joint_name
;
const
VectorXs
&
m_max_effort
;
const
VectorXs
&
m_max_velocity
;
const
VectorXs
&
m_min_config
;
const
VectorXs
&
m_max_config
;
addJoint
WithLimits
Visitor
(
Model
&
model
,
addJointVisitor
1
(
Model
&
model
,
const
JointIndex
parent_id
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
,
...
...
@@ -142,10 +143,7 @@ namespace pinocchio
const
VectorXs
&
max_velocity
,
const
VectorXs
&
min_config
,
const
VectorXs
&
max_config
)
:
m_model
(
model
)
,
m_parent_id
(
parent_id
)
,
m_joint_placement
(
joint_placement
)
,
m_joint_name
(
joint_name
)
:
addJointVisitor0
(
model
,
parent_id
,
joint_placement
,
joint_name
)
,
m_max_effort
(
max_effort
)
,
m_max_velocity
(
max_velocity
)
,
m_min_config
(
min_config
)
...
...
@@ -157,7 +155,45 @@ namespace pinocchio
{
return
m_model
.
addJoint
(
m_parent_id
,
jmodel
,
m_joint_placement
,
m_joint_name
,
m_max_effort
,
m_max_velocity
,
m_min_config
,
m_max_config
);
}
};
// struct addJointWithLimitsVisitor
};
// struct addJointVisitor1
struct
addJointVisitor2
:
public
addJointVisitor1
{
using
addJointVisitor1
::
m_model
;
using
addJointVisitor1
::
m_parent_id
;
using
addJointVisitor1
::
m_joint_placement
;
using
addJointVisitor1
::
m_joint_name
;
using
addJointVisitor1
::
m_max_effort
;
using
addJointVisitor1
::
m_max_velocity
;
using
addJointVisitor1
::
m_min_config
;
using
addJointVisitor1
::
m_max_config
;
const
VectorXs
&
m_friction
;
const
VectorXs
&
m_damping
;
addJointVisitor2
(
Model
&
model
,
const
JointIndex
parent_id
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
,
const
VectorXs
&
max_effort
,
const
VectorXs
&
max_velocity
,
const
VectorXs
&
min_config
,
const
VectorXs
&
max_config
,
const
VectorXs
&
friction
,
const
VectorXs
&
damping
)
:
addJointVisitor1
(
model
,
parent_id
,
joint_placement
,
joint_name
,
max_effort
,
max_velocity
,
min_config
,
max_config
)
,
m_friction
(
friction
)
,
m_damping
(
damping
)
{}
template
<
typename
JointModelDerived
>
JointIndex
operator
()(
JointModelDerived
&
jmodel
)
const
{
return
m_model
.
addJoint
(
m_parent_id
,
jmodel
,
m_joint_placement
,
m_joint_name
,
m_max_effort
,
m_max_velocity
,
m_min_config
,
m_max_config
,
m_friction
,
m_damping
);
}
};
// struct addJointVisitor1
public:
...
...
@@ -191,6 +227,10 @@ namespace pinocchio
"Vector of rotor inertia parameters."
)
.
def_readwrite
(
"rotorGearRatio"
,
&
Model
::
rotorGearRatio
,
"Vector of rotor gear ratio parameters."
)
.
def_readwrite
(
"friction"
,
&
Model
::
friction
,
"Vector of joint friction parameters."
)
.
def_readwrite
(
"damping"
,
&
Model
::
damping
,
"Vector of joint damping parameters."
)
.
def_readwrite
(
"effortLimit"
,
&
Model
::
effortLimit
,
"Joint max effort."
)
.
def_readwrite
(
"velocityLimit"
,
&
Model
::
velocityLimit
,
...
...
@@ -216,13 +256,21 @@ namespace pinocchio
"Motion vector corresponding to the gravity field expressed in the world Frame."
)
// Class Methods
.
def
(
"addJoint"
,
&
ModelPythonVisitor
::
addJoint
,
.
def
(
"addJoint"
,
&
ModelPythonVisitor
::
addJoint
0
,
bp
::
args
(
"self"
,
"parent_id"
,
"joint_model"
,
"joint_placement"
,
"joint_name"
),
"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name."
)
.
def
(
"addJoint"
,
&
ModelPythonVisitor
::
addJoint
WithLimits
,
.
def
(
"addJoint"
,
&
ModelPythonVisitor
::
addJoint
1
,
bp
::
args
(
"self"
,
"parent_id"
,
"joint_model"
,
"joint_placement"
,
"joint_name"
,
"max_effort"
,
"max_velocity"
,
"min_config"
,
"max_config"
),
"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name."
)
"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name."
"This signature also takes as input effort, velocity limits as well as the bounds on the joint configuration."
)
.
def
(
"addJoint"
,
&
ModelPythonVisitor
::
addJoint2
,
bp
::
args
(
"self"
,
"parent_id"
,
"joint_model"
,
"joint_placement"
,
"joint_name"
,
"max_effort"
,
"max_velocity"
,
"min_config"
,
"max_config"
,
"friction"
,
"damping"
),
"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name.
\n
"
"This signature also takes as input effort, velocity limits as well as the bounds on the joint configuration.
\n
"
"The user should also provide the friction and damping related to the joint."
)
.
def
(
"addJointFrame"
,
&
Model
::
addJointFrame
,
addJointFrame_overload
(
bp
::
args
(
"self"
,
"joint_id"
,
"frame_id"
),
"Add the joint provided by its joint_id as a frame to the frame tree.
\n
"
...
...
@@ -255,28 +303,44 @@ namespace pinocchio
;
}
static
JointIndex
addJoint
(
Model
&
model
,
JointIndex
parent_id
,
bp
::
object
jmodel
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
)
static
JointIndex
addJoint
0
(
Model
&
model
,
JointIndex
parent_id
,
bp
::
object
jmodel
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
)
{
JointModelVariant
jmodel_variant
=
bp
::
extract
<
JointModelVariant
>
(
jmodel
)();
return
boost
::
apply_visitor
(
addJointVisitor
(
model
,
parent_id
,
joint_placement
,
joint_name
),
jmodel_variant
);
return
boost
::
apply_visitor
(
addJointVisitor
0
(
model
,
parent_id
,
joint_placement
,
joint_name
),
jmodel_variant
);
}
static
JointIndex
addJointWithLimits
(
Model
&
model
,
JointIndex
parent_id
,
bp
::
object
jmodel
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
,
const
VectorXs
&
max_effort
,
const
VectorXs
&
max_velocity
,
const
VectorXs
&
min_config
,
const
VectorXs
&
max_config
)
static
JointIndex
addJoint1
(
Model
&
model
,
JointIndex
parent_id
,
bp
::
object
jmodel
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
,
const
VectorXs
&
max_effort
,
const
VectorXs
&
max_velocity
,
const
VectorXs
&
min_config
,
const
VectorXs
&
max_config
)
{
JointModelVariant
jmodel_variant
=
bp
::
extract
<
JointModelVariant
>
(
jmodel
)();
return
boost
::
apply_visitor
(
addJointVisitor1
(
model
,
parent_id
,
joint_placement
,
joint_name
,
max_effort
,
max_velocity
,
min_config
,
max_config
),
jmodel_variant
);
}
static
JointIndex
addJoint2
(
Model
&
model
,
JointIndex
parent_id
,
bp
::
object
jmodel
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
,
const
VectorXs
&
max_effort
,
const
VectorXs
&
max_velocity
,
const
VectorXs
&
min_config
,
const
VectorXs
&
max_config
,
const
VectorXs
&
friction
,
const
VectorXs
&
damping
)
{
JointModelVariant
jmodel_variant
=
bp
::
extract
<
JointModelVariant
>
(
jmodel
)();
return
boost
::
apply_visitor
(
addJoint
WithLimits
Visitor
(
model
,
parent_id
,
joint_placement
,
joint_name
,
max_effort
,
max_velocity
,
min_config
,
max_config
),
jmodel_variant
);
return
boost
::
apply_visitor
(
addJointVisitor
2
(
model
,
parent_id
,
joint_placement
,
joint_name
,
max_effort
,
max_velocity
,
min_config
,
max_config
,
friction
,
damping
),
jmodel_variant
);
}
static
Data
createData
(
const
Model
&
model
)
{
return
Data
(
model
);
}
...
...
src/algorithm/model.hxx
View file @
aa39638f
...
...
@@ -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
);
}
}
}
...
...
@@ -384,11 +391,20 @@ namespace pinocchio
joint_input_model
.
jointVelocitySelector
(
input_model
.
effortLimit
),
joint_input_model
.
jointVelocitySelector
(
input_model
.
velocityLimit
),
joint_input_model
.
jointConfigSelector
(
input_model
.
lowerPositionLimit
),
joint_input_model
.
jointConfigSelector
(
input_model
.
upperPositionLimit
));
joint_input_model
.
jointConfigSelector
(
input_model
.
upperPositionLimit
),
joint_input_model
.
jointVelocitySelector
(
input_model
.
friction
),
joint_input_model
.
jointVelocitySelector
(
input_model
.
damping
));
// Append inertia
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
);
}
}
...
...
src/multibody/model.hpp
View file @
aa39638f
...
...
@@ -65,6 +65,7 @@ namespace pinocchio
/// \brief Dense vectorized version of a joint configuration vector.
typedef
VectorXs
ConfigVectorType
;
/// \brief Map between a string (key) and a configuration vector
typedef
std
::
map
<
std
::
string
,
ConfigVectorType
>
ConfigVectorMap
;
/// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc).
...
...
@@ -117,11 +118,17 @@ namespace pinocchio
ConfigVectorMap
referenceConfigurations
;
/// \brief Vector of rotor inertia parameters
Vector
Xs
rotorInertia
;
Tangent
Vector
Type
rotorInertia
;
/// \brief Vector of rotor gear ratio parameters
Vector
Xs
rotorGearRatio
;
Tangent
Vector
Type
rotorGearRatio
;
/// \brief Vector of joint friction parameters
TangentVectorType
friction
;
/// \brief Vector of joint damping parameters
TangentVectorType
damping
;
/// \brief Vector of maximal joint torques
TangentVectorType
effortLimit
;
...
...
@@ -129,10 +136,10 @@ namespace pinocchio
TangentVectorType
velocityLimit
;
/// \brief Lower joint configuration limit
Vector
Xs
lowerPositionLimit
;
Config
Vector
Type
lowerPositionLimit
;
/// \brief Upper joint configuration limit
Vector
Xs
upperPositionLimit
;
Config
Vector
Type
upperPositionLimit
;
/// \brief Vector of operational frames registered on the model.
FrameVector
frames
;
...
...
@@ -208,6 +215,8 @@ namespace pinocchio
// Eigen Vectors
res
.
rotorInertia
=
rotorInertia
.
template
cast
<
NewScalar
>();
res
.
rotorGearRatio
=
rotorGearRatio
.
template
cast
<
NewScalar
>();
res
.
friction
=
friction
.
template
cast
<
NewScalar
>();
res
.
damping
=
damping
.
template
cast
<
NewScalar
>();
res
.
effortLimit
=
effortLimit
.
template
cast
<
NewScalar
>();
res
.
velocityLimit
=
velocityLimit
.
template
cast
<
NewScalar
>();
res
.
lowerPositionLimit
=
lowerPositionLimit
.
template
cast
<
NewScalar
>();
...
...
@@ -287,6 +296,16 @@ namespace pinocchio
res
&=
other
.
rotorInertia
==
rotorInertia
;
if
(
!
res
)
return
res
;
if
(
other
.
friction
.
size
()
!=
friction
.
size
())
return
false
;
res
&=
other
.
friction
==
friction
;
if
(
!
res
)
return
res
;
if
(
other
.
damping
.
size
()
!=
damping
.
size
())
return
false
;
res
&=
other
.
damping
==
damping
;
if
(
!
res
)
return
res
;
if
(
other
.
rotorGearRatio
.
size
()
!=
rotorGearRatio
.
size
())
return
false
;
res
&=
other
.
rotorGearRatio
==
rotorGearRatio
;
...
...
@@ -336,12 +355,11 @@ namespace pinocchio
///
bool
operator
!=
(
const
ModelTpl
&
other
)
const
{
return
!
(
*
this
==
other
);
}
///
/// \brief Add a joint to the kinematic tree with
given
bounds.
/// \brief Add a joint to the kinematic tree with
infinite
bounds.
///
/// \remark This method does not add a Frame of same name to the vector of frames.
/// Use Model::addJointFrame.
/// \remark This method also adds a Frame of same name to the vector of frames.
/// \remark The inertia supported by the joint is set to Zero.
///
/// \tparam JointModelDerived The type of the joint model.
...
...
@@ -350,15 +368,24 @@ namespace pinocchio
/// \param[in] joint_model The joint model.
/// \param[in] joint_placement Placement of the joint inside its parent joint.
/// \param[in] joint_name Name of the joint. If empty, the name is random.
///
/// \return The index of the new joint.
///
/// \sa Model::appendBodyToJoint
///
JointIndex
addJoint
(
const
JointIndex
parent
,
const
JointModel
&
joint_model
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
);
///
/// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const std::string &)
///
/// \param[in] max_effort Maximal joint torque.
/// \param[in] max_velocity Maximal joint velocity.
/// \param[in] min_config Lower joint configuration.
/// \param[in] max_config Upper joint configuration.
///
/// \return The index of the new joint.
///
/// \sa Model::appendBodyToJoint, Model::addJointFrame
///
JointIndex
addJoint
(
const
JointIndex
parent
,
const
JointModel
&
joint_model
,
const
SE3
&
joint_placement
,
...
...
@@ -366,30 +393,24 @@ namespace pinocchio
const
VectorXs
&
max_effort
,
const
VectorXs
&
max_velocity
,
const
VectorXs
&
min_config
,
const
VectorXs
&
max_config
);
///
/// \brief Add a joint to the kinematic tree with infinite bounds.
///
/// \remark This method also adds a Frame of same name to the vector of frames.