Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
P
pinocchio
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Stack Of Tasks
pinocchio
Commits
818f7aae
Commit
818f7aae
authored
8 years ago
by
Joseph Mirabel
Committed by
Joseph Mirabel
8 years ago
Browse files
Options
Downloads
Patches
Plain Diff
[C++] Add Frame::previousFrame and update class Model
parent
21bb5b85
No related branches found
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
src/multibody/frame.hpp
+7
-1
7 additions, 1 deletion
src/multibody/frame.hpp
src/multibody/model.hpp
+33
-5
33 additions, 5 deletions
src/multibody/model.hpp
src/multibody/model.hxx
+28
-2
28 additions, 2 deletions
src/multibody/model.hxx
with
68 additions
and
8 deletions
src/multibody/frame.hpp
+
7
−
1
View file @
818f7aae
...
@@ -58,12 +58,14 @@ namespace se3
...
@@ -58,12 +58,14 @@ namespace se3
///
///
/// \param[in] name Name of the frame.
/// \param[in] name Name of the frame.
/// \param[in] parent Index of the parent joint in the kinematic tree.
/// \param[in] parent Index of the parent joint in the kinematic tree.
/// \param[in] previousFrame Index of the parent frame in the kinematic tree.
/// \param[in] placement Placement of the frame wrt the parent joint frame.
/// \param[in] placement Placement of the frame wrt the parent joint frame.
/// \param[in] type The type of the frame, see the enum FrameType
/// \param[in] type The type of the frame, see the enum FrameType
///
///
Frame
(
const
std
::
string
&
name
,
const
JointIndex
parent
,
const
SE3
&
frame_placement
,
const
FrameType
type
)
Frame
(
const
std
::
string
&
name
,
const
JointIndex
parent
,
const
FrameIndex
previousFrame
,
const
SE3
&
frame_placement
,
const
FrameType
type
)
:
name
(
name
)
:
name
(
name
)
,
parent
(
parent
)
,
parent
(
parent
)
,
previousFrame
(
previousFrame
)
,
placement
(
frame_placement
)
,
placement
(
frame_placement
)
,
type
(
type
)
,
type
(
type
)
{}
{}
...
@@ -76,6 +78,7 @@ namespace se3
...
@@ -76,6 +78,7 @@ namespace se3
bool
operator
==
(
const
Frame
&
other
)
const
bool
operator
==
(
const
Frame
&
other
)
const
{
{
return
name
==
other
.
name
&&
parent
==
other
.
parent
return
name
==
other
.
name
&&
parent
==
other
.
parent
&&
previousFrame
==
other
.
previousFrame
&&
placement
==
other
.
placement
&&
placement
==
other
.
placement
&&
type
==
other
.
type
;
&&
type
==
other
.
type
;
}
}
...
@@ -86,6 +89,9 @@ namespace se3
...
@@ -86,6 +89,9 @@ namespace se3
/// \brief Index of the parent joint.
/// \brief Index of the parent joint.
JointIndex
parent
;
JointIndex
parent
;
/// \brief Index of the previous frame.
FrameIndex
previousFrame
;
/// \brief Placement of the frame wrt the parent joint.
/// \brief Placement of the frame wrt the parent joint.
SE3
placement
;
SE3
placement
;
...
...
This diff is collapsed.
Click to expand it.
src/multibody/model.hpp
+
33
−
5
View file @
818f7aae
...
@@ -124,13 +124,15 @@ namespace se3
...
@@ -124,13 +124,15 @@ namespace se3
,
gravity
(
gravity981
,
Eigen
::
Vector3d
::
Zero
()
)
,
gravity
(
gravity981
,
Eigen
::
Vector3d
::
Zero
()
)
{
{
names
[
0
]
=
"universe"
;
// Should be "universe joint (trivial)"
names
[
0
]
=
"universe"
;
// Should be "universe joint (trivial)"
addFrame
(
Frame
(
"universe"
,
0
,
0
,
SE3
::
Identity
(),
FIXED_JOINT
));
}
}
~
Model
()
{}
// std::cout << "Destroy model" << std::endl; }
~
Model
()
{}
// std::cout << "Destroy model" << std::endl; }
///
///
/// \brief Add a joint to the kinematic tree.
/// \brief Add a joint to the kinematic tree.
///
///
/// \remark This method also adds a Frame of same name to the vector of frames.
/// \remark This method does not add a Frame of same name to the vector of frames.
/// Use Model::addJointFrame.
/// \remark The inertia supported by the joint is set to Zero.
/// \remark The inertia supported by the joint is set to Zero.
///
///
/// \tparam JointModelDerived The type of the joint model.
/// \tparam JointModelDerived The type of the joint model.
...
@@ -146,7 +148,7 @@ namespace se3
...
@@ -146,7 +148,7 @@ namespace se3
///
///
/// \return The index of the new joint.
/// \return The index of the new joint.
///
///
/// \sa Model::appendBodyToJoint
/// \sa Model::appendBodyToJoint
, Model::addJointFrame
///
///
template
<
typename
JointModelDerived
>
template
<
typename
JointModelDerived
>
JointIndex
addJoint
(
const
JointIndex
parent
,
const
JointModelBase
<
JointModelDerived
>
&
joint_model
,
const
SE3
&
joint_placement
,
JointIndex
addJoint
(
const
JointIndex
parent
,
const
JointModelBase
<
JointModelDerived
>
&
joint_model
,
const
SE3
&
joint_placement
,
...
@@ -157,6 +159,18 @@ namespace se3
...
@@ -157,6 +159,18 @@ namespace se3
const
Eigen
::
VectorXd
&
max_config
=
Eigen
::
VectorXd
::
Constant
(
JointModelDerived
::
NQ
,
std
::
numeric_limits
<
double
>::
max
())
const
Eigen
::
VectorXd
&
max_config
=
Eigen
::
VectorXd
::
Constant
(
JointModelDerived
::
NQ
,
std
::
numeric_limits
<
double
>::
max
())
);
);
///
/// \brief Add a joint to the frame tree.
///
/// \param[in] jointIndex Index of the joint.
/// \param[in] frameIndex Index of the parent frame. If negative,
/// the parent frame is the frame of the parent joint.
///
/// \return The index of the new frame.
///
FrameIndex
addJointFrame
(
const
JointIndex
&
jointIndex
,
int
frameIndex
=
-
1
);
///
///
/// \brief Append a body to a given joint of the kinematic tree.
/// \brief Append a body to a given joint of the kinematic tree.
///
///
...
@@ -165,13 +179,27 @@ namespace se3
...
@@ -165,13 +179,27 @@ namespace se3
/// \param[in] joint_index Index of the supporting joint.
/// \param[in] joint_index Index of the supporting joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
/// \param[in] body_name Name of the body. If empty, the name is random.
///
///
/// \sa Model::addJoint
/// \sa Model::addJoint
///
///
void
appendBodyToJoint
(
const
JointIndex
joint_index
,
const
Inertia
&
Y
,
void
appendBodyToJoint
(
const
JointIndex
joint_index
,
const
Inertia
&
Y
,
const
SE3
&
body_placement
=
SE3
::
Identity
(),
const
SE3
&
body_placement
=
SE3
::
Identity
());
const
std
::
string
&
body_name
=
""
);
///
/// \brief Add a body to the frame tree.
///
/// \param[in] body_name Name of the body.
/// \param[in] parentJoint Index of the parent joint.
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
/// \param[in] previousFrame Index of the parent frame. If negative,
/// the parent frame is the frame of the parent joint.
///
/// \return The index of the new frame.
///
FrameIndex
addBodyFrame
(
const
std
::
string
&
body_name
,
const
JointIndex
&
parentJoint
,
const
SE3
&
body_placement
=
SE3
::
Identity
(),
int
previousFrame
=
-
1
);
///
///
/// \brief Return the index of a body given by its name.
/// \brief Return the index of a body given by its name.
...
...
This diff is collapsed.
Click to expand it.
src/multibody/model.hxx
+
28
−
2
View file @
818f7aae
...
@@ -87,15 +87,41 @@ namespace se3
...
@@ -87,15 +87,41 @@ namespace se3
return
idx
;
return
idx
;
}
}
inline
FrameIndex
Model
::
addJointFrame
(
const
JointIndex
&
jidx
,
int
fidx
)
{
if
(
fidx
<
0
)
{
fidx
=
getFrameId
(
names
[
parents
[
jidx
]]);
}
if
(
fidx
>=
frames
.
size
())
throw
std
::
invalid_argument
(
"Frame not found"
);
// Add a the joint frame attached to itself to the frame vector - redundant information but useful.
addFrame
(
Frame
(
names
[
jidx
],
jidx
,
fidx
,
SE3
::
Identity
(),
JOINT
));
return
frames
.
size
()
-
1
;
}
inline
void
Model
::
appendBodyToJoint
(
const
Model
::
JointIndex
joint_index
,
inline
void
Model
::
appendBodyToJoint
(
const
Model
::
JointIndex
joint_index
,
const
Inertia
&
Y
,
const
Inertia
&
Y
,
const
SE3
&
body_placement
,
const
SE3
&
body_placement
)
const
std
::
string
&
body_name
)
{
{
const
Inertia
&
iYf
=
Y
.
se3Action
(
body_placement
);
const
Inertia
&
iYf
=
Y
.
se3Action
(
body_placement
);
inertias
[
joint_index
]
+=
iYf
;
inertias
[
joint_index
]
+=
iYf
;
nbody
++
;
nbody
++
;
}
}
inline
FrameIndex
Model
::
addBodyFrame
(
const
std
::
string
&
body_name
,
const
JointIndex
&
parentJoint
,
const
SE3
&
body_placement
,
int
previousFrame
)
{
if
(
previousFrame
<
0
)
{
previousFrame
=
getFrameId
(
names
[
parentJoint
]);
}
if
(
previousFrame
>=
frames
.
size
())
throw
std
::
invalid_argument
(
"Frame not found"
);
addFrame
(
Frame
(
body_name
,
parentJoint
,
previousFrame
,
body_placement
,
BODY
));
return
frames
.
size
()
-
1
;
}
inline
Model
::
JointIndex
Model
::
getBodyId
(
const
std
::
string
&
name
)
const
inline
Model
::
JointIndex
Model
::
getBodyId
(
const
std
::
string
&
name
)
const
{
{
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment