Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-model
Commits
f06ac96d
Commit
f06ac96d
authored
Feb 04, 2015
by
Joseph Mirabel
Committed by
Joseph Mirabel
Feb 20, 2015
Browse files
Add class CenterOfMassComputation
parent
deeb1568
Changes
6
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
f06ac96d
...
...
@@ -59,6 +59,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/model/object-factory.hh
include/hpp/model/object-iterator.hh
include/hpp/model/gripper.hh
include/hpp/model/center-of-mass-computation.hh
)
# Declare dependencies
...
...
include/hpp/model/center-of-mass-computation.hh
0 → 100644
View file @
f06ac96d
// Copyright (c) 2015, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-model.
// hpp-model is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-model is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-model. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_MODEL_CENTER_OF_MASS_COMPUTATION_HH
# define HPP_MODEL_CENTER_OF_MASS_COMPUTATION_HH
# include <list>
# include <hpp/model/fwd.hh>
# include <hpp/model/device.hh>
namespace
hpp
{
namespace
model
{
class
CenterOfMassComputation
{
public:
static
CenterOfMassComputationPtr_t
create
(
const
DevicePtr_t
&
device
);
void
add
(
const
JointPtr_t
&
joint
);
void
compute
(
const
Device
::
Computation_t
&
flag
=
Device
::
ALL
);
const
fcl
::
Vec3f
&
com
()
const
{
return
com_
;
}
const
value_type
&
mass
()
const
{
return
mass_
;
}
void
computeMass
();
const
ComJacobian_t
&
jacobian
()
const
{
return
jacobianCom_
;
}
~
CenterOfMassComputation
();
protected:
CenterOfMassComputation
(
const
DevicePtr_t
&
device
);
private:
// Keep a tree structure in order to compute a partial COM
struct
JointTreeElement_t
;
struct
isJoint
;
typedef
std
::
list
<
JointTreeElement_t
*>
JointTreeList
;
// JointTreeElement_t s that have no parents
JointTreeList
rootJointTrees_
;
// all JointTreeElement_t s
JointTreeList
jointTrees_
;
value_type
mass_
;
vector3_t
massCom_
;
vector3_t
com_
;
ComJacobian_t
jacobianCom_
;
};
// class CenterOfMassComputation
}
// namespace model
}
// namespace hpp
#endif // HPP_MODEL_CENTER_OF_MASS_COMPUTATION_HH
include/hpp/model/fwd.hh
View file @
f06ac96d
...
...
@@ -44,6 +44,7 @@ namespace hpp {
HPP_PREDEF_CLASS
(
ObjectFactory
);
HPP_PREDEF_CLASS
(
ObjectIterator
);
HPP_PREDEF_CLASS
(
Gripper
);
HPP_PREDEF_CLASS
(
CenterOfMassComputation
);
enum
Request_t
{
COLLISION
,
DISTANCE
};
typedef
double
value_type
;
...
...
@@ -92,6 +93,8 @@ namespace hpp {
typedef
boost
::
shared_ptr
<
Gripper
>
GripperPtr_t
;
typedef
std
::
vector
<
GripperPtr_t
>
Grippers_t
;
typedef
fcl
::
Transform3f
Transform3f
;
typedef
boost
::
shared_ptr
<
CenterOfMassComputation
>
CenterOfMassComputationPtr_t
;
}
// namespace model
}
// namespace hpp
#endif //HPP_MODEL_FWD_HH
include/hpp/model/joint.hh
View file @
f06ac96d
...
...
@@ -334,6 +334,7 @@ namespace hpp {
std
::
size_t
rankInParent_
;
friend
class
Device
;
friend
class
ChildrenIterator
;
friend
class
CenterOfMassComputation
;
};
// class Joint
/// Anchor Joint
...
...
src/CMakeLists.txt
View file @
f06ac96d
...
...
@@ -29,6 +29,7 @@ ADD_LIBRARY(${LIBRARY_NAME}
joint-configuration.cc
object-iterator.cc
gripper.cc
center-of-mass-computation.cc
)
PKG_CONFIG_USE_DEPENDENCY
(
${
LIBRARY_NAME
}
hpp-util
)
...
...
src/center-of-mass-computation.cc
0 → 100644
View file @
f06ac96d
// Copyright (c) 2015, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-model.
// hpp-model is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-model is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-model. If not, see <http://www.gnu.org/licenses/>.
#include
"hpp/model/center-of-mass-computation.hh"
#include
<algorithm>
#include
<queue>
#include
"hpp/model/joint.hh"
#include
"hpp/model/device.hh"
namespace
hpp
{
namespace
model
{
struct
CenterOfMassComputation
::
JointTreeElement_t
{
JointTreeElement_t
*
parent
;
JointPtr_t
j
;
fcl
::
Vec3f
massCom_
;
std
::
vector
<
JointTreeElement_t
*>
children
;
JointTreeElement_t
(
const
JointPtr_t
&
joint
)
:
parent
(
NULL
),
j
(
joint
),
children
()
{
massCom_
.
setValue
(
0
);
}
};
struct
CenterOfMassComputation
::
isJoint
{
JointPtr_t
j
;
isJoint
(
const
JointPtr_t
&
jt
)
:
j
(
jt
)
{}
bool
operator
()
(
const
JointTreeElement_t
*
e
)
{
return
e
->
j
==
j
;
}
};
CenterOfMassComputationPtr_t
CenterOfMassComputation
::
create
(
const
DevicePtr_t
&
d
)
{
return
CenterOfMassComputationPtr_t
(
new
CenterOfMassComputation
(
d
));
}
void
CenterOfMassComputation
::
computeMass
()
{
mass_
=
0
;
for
(
JointTreeList
::
iterator
it
=
rootJointTrees_
.
begin
();
it
!=
rootJointTrees_
.
end
();
++
it
)
{
mass_
+=
(
*
it
)
->
j
->
computeMass
();
}
assert
(
mass_
>
0
);
}
void
CenterOfMassComputation
::
compute
(
const
Device
::
Computation_t
&
flag
)
{
assert
(
mass_
>
0
);
if
(
flag
|
Device
::
COM
)
{
massCom_
.
setValue
(
0
);
for
(
JointTreeList
::
iterator
it
=
rootJointTrees_
.
begin
();
it
!=
rootJointTrees_
.
end
();
++
it
)
{
(
*
it
)
->
j
->
computeMassTimesCenterOfMass
();
massCom_
+=
(
*
it
)
->
j
->
massCom_
;
}
com_
=
massCom_
/
mass_
;
}
if
(
flag
|
Device
::
JACOBIAN
)
{
jacobianCom_
.
setZero
();
for
(
JointTreeList
::
iterator
it
=
jointTrees_
.
begin
();
it
!=
jointTrees_
.
end
();
++
it
)
(
*
it
)
->
j
->
writeComSubjacobian
(
jacobianCom_
,
mass_
);
}
}
CenterOfMassComputation
::
CenterOfMassComputation
(
const
DevicePtr_t
&
d
)
:
rootJointTrees_
(),
jointTrees_
(),
mass_
(
-
1
),
jacobianCom_
(
3
,
d
->
numberDof
())
{
massCom_
.
setZero
();
}
void
CenterOfMassComputation
::
add
(
const
JointPtr_t
&
j
)
{
JointTreeList
::
iterator
itj
=
std
::
find_if
(
jointTrees_
.
begin
(),
jointTrees_
.
end
(),
isJoint
(
j
));
if
(
itj
==
jointTrees_
.
end
())
{
// TODO: Here we consider that all added joint are root joints. It
// means that you must consider all joints below this joints.
// A tree structure should be built to allow a finer selection of
// joints.
JointTreeElement_t
*
jtree
=
new
JointTreeElement_t
(
j
);
rootJointTrees_
.
push_back
(
jtree
);
jointTrees_
.
push_back
(
jtree
);
std
::
queue
<
JointTreeElement_t
*>
q
;
q
.
push
(
jtree
);
while
(
!
q
.
empty
())
{
JointTreeElement_t
*
current
=
q
.
front
();
for
(
size_t
r
=
0
;
r
<
current
->
j
->
numberChildJoints
();
++
r
)
{
JointTreeElement_t
*
next
=
new
JointTreeElement_t
(
current
->
j
->
childJoint
(
r
));
next
->
parent
=
current
;
current
->
children
.
push_back
(
next
);
jointTrees_
.
push_back
(
next
);
q
.
push
(
next
);
}
q
.
pop
();
}
}
}
CenterOfMassComputation
::~
CenterOfMassComputation
()
{
for
(
JointTreeList
::
iterator
it
=
jointTrees_
.
begin
();
it
!=
jointTrees_
.
end
();)
{
delete
*
it
;
it
=
jointTrees_
.
erase
(
it
);
}
}
}
// namespace model
}
// namespace hpp
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a 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