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
fa693f2f
Verified
Commit
fa693f2f
authored
Oct 02, 2020
by
Justin Carpentier
Browse files
core: handle friction and damping parameters when adding joint
parent
b7628d41
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/multibody/model.hpp
View file @
fa693f2f
...
...
@@ -354,12 +354,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.
...
...
@@ -368,15 +367,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
,
...
...
@@ -384,30 +392,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.
/// \remark The inertia supported by the joint is set to Zero.
///
/// \tparam JointModelDerived The type of the joint model.
///
/// \param[in] parent Index of the parent joint.
/// \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.
const
VectorXs
&
max_config
);
///
/// \
return The index of the new joint.
/// \
copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const std::string &,const VectorXs &,const VectorXs &,const VectorXs &,const VectorXs &)
///
/// \sa Model::appendBodyToJoint
/// \param[in] friction Joint friction parameters.
/// \param[in] damping Joint damping parameters.
///
JointIndex
addJoint
(
const
JointIndex
parent
,
const
JointModel
&
joint_model
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
);
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
);
///
/// \brief Add a joint to the frame tree.
...
...
src/multibody/model.hxx
View file @
fa693f2f
...
...
@@ -63,7 +63,9 @@ namespace pinocchio
const
VectorXs
&
max_effort
,
const
VectorXs
&
max_velocity
,
const
VectorXs
&
min_config
,
const
VectorXs
&
max_config
const
VectorXs
&
max_config
,
const
VectorXs
&
joint_friction
,
const
VectorXs
&
joint_damping
)
{
assert
(
(
njoints
==
(
int
)
joints
.
size
())
&&
(
njoints
==
(
int
)
inertias
.
size
())
...
...
@@ -71,11 +73,13 @@ namespace pinocchio
assert
((
joint_model
.
nq
()
>=
0
)
&&
(
joint_model
.
nv
()
>=
0
));
assert
(
joint_model
.
nq
()
>=
joint_model
.
nv
());
assert
(
max_effort
.
size
()
==
joint_model
.
nv
()
&&
max_velocity
.
size
()
==
joint_model
.
nv
()
&&
min_config
.
size
()
==
joint_model
.
nq
()
&&
max_config
.
size
()
==
joint_model
.
nq
());
PINOCCHIO_CHECK_ARGUMENT_SIZE
(
max_effort
.
size
(),
joint_model
.
nv
(),
"The joint maximum effort vector is not of right size"
);
PINOCCHIO_CHECK_ARGUMENT_SIZE
(
max_velocity
.
size
(),
joint_model
.
nv
(),
"The joint maximum velocity vector is not of right size"
);
PINOCCHIO_CHECK_ARGUMENT_SIZE
(
min_config
.
size
(),
joint_model
.
nq
(),
"The joint lower configuration bound is not of right size"
);
PINOCCHIO_CHECK_ARGUMENT_SIZE
(
max_config
.
size
(),
joint_model
.
nq
(),
"The joint upper configuration bound is not of right size"
);
PINOCCHIO_CHECK_ARGUMENT_SIZE
(
joint_friction
.
size
(),
joint_model
.
nv
(),
"The joint friction vector is not of right size"
);
PINOCCHIO_CHECK_ARGUMENT_SIZE
(
joint_damping
.
size
(),
joint_model
.
nv
(),
"The joint damping vector is not of right size"
);
JointIndex
idx
=
(
JointIndex
)(
njoints
++
);
joints
.
push_back
(
JointModel
(
joint_model
.
derived
()));
...
...
@@ -114,6 +118,10 @@ namespace pinocchio
jmodel
.
jointVelocitySelector
(
rotorInertia
).
setZero
();
rotorGearRatio
.
conservativeResize
(
nv
);
jmodel
.
jointVelocitySelector
(
rotorGearRatio
).
setOnes
();
friction
.
conservativeResize
(
nv
);
jmodel
.
jointVelocitySelector
(
friction
)
=
joint_friction
;
damping
.
conservativeResize
(
nv
);
jmodel
.
jointVelocitySelector
(
damping
)
=
joint_damping
;
}
// Init and add joint index to its parent subtrees.
...
...
@@ -127,6 +135,26 @@ namespace pinocchio
return
idx
;
}
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
>
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
addJoint
(
const
JointIndex
parent
,
const
JointModel
&
joint_model
,
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
=
VectorXs
::
Constant
(
joint_model
.
nv
(),
static_cast
<
Scalar
>
(
0
));
const
VectorXs
damping
=
VectorXs
::
Constant
(
joint_model
.
nv
(),
static_cast
<
Scalar
>
(
0
));
return
addJoint
(
parent
,
joint_model
,
joint_placement
,
joint_name
,
max_effort
,
max_velocity
,
min_config
,
max_config
,
friction
,
damping
);
}
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
>
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
...
...
@@ -135,14 +163,13 @@ namespace pinocchio
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
)
{
VectorXs
max_effort
,
max_velocity
,
min_config
,
max_config
;
max_effort
=
VectorXs
::
Constant
(
joint_model
.
nv
(),
std
::
numeric_limits
<
Scalar
>::
max
());
max_velocity
=
VectorXs
::
Constant
(
joint_model
.
nv
(),
std
::
numeric_limits
<
Scalar
>::
max
());
min_config
=
VectorXs
::
Constant
(
joint_model
.
nq
(),
-
std
::
numeric_limits
<
Scalar
>::
max
());
max_config
=
VectorXs
::
Constant
(
joint_model
.
nq
(),
std
::
numeric_limits
<
Scalar
>::
max
());
const
VectorXs
max_effort
=
VectorXs
::
Constant
(
joint_model
.
nv
(),
std
::
numeric_limits
<
Scalar
>::
max
());
const
VectorXs
max_velocity
=
VectorXs
::
Constant
(
joint_model
.
nv
(),
std
::
numeric_limits
<
Scalar
>::
max
());
const
VectorXs
min_config
=
VectorXs
::
Constant
(
joint_model
.
nq
(),
-
std
::
numeric_limits
<
Scalar
>::
max
());
const
VectorXs
max_config
=
VectorXs
::
Constant
(
joint_model
.
nq
(),
std
::
numeric_limits
<
Scalar
>::
max
());
return
addJoint
(
parent
,
joint_model
,
joint_placement
,
joint_name
,
max_effort
,
max_velocity
,
min_config
,
max_config
);
return
addJoint
(
parent
,
joint_model
,
joint_placement
,
joint_name
,
max_effort
,
max_velocity
,
min_config
,
max_config
);
}
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
>
...
...
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