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
sot-dynamic-pinocchio
Commits
de07e548
Commit
de07e548
authored
Dec 25, 2010
by
florent
Browse files
Add commands to build a robot
* include/sot-dynamic/dynamic.h, * src/dynamic-command.h, * src/dynamic.cpp.
parent
5f67c35e
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/sot-dynamic/dynamic.h
View file @
de07e548
...
...
@@ -25,6 +25,10 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* STD */
#include
<string>
#include
<map>
/* Matrix */
#include
<jrl/mal/boost.hh>
#include
"jrl/mal/matrixabstractlayer.hh"
...
...
@@ -33,6 +37,7 @@ namespace ml = maal::boost;
/* JRL dynamic */
#include
<abstract-robot-dynamics/humanoid-dynamic-robot.hh>
#include
<jrl/dynamics/dynamicsfactory.hh>
namespace
djj
=
dynamicsJRLJapan
;
/* SOT */
...
...
@@ -44,9 +49,6 @@ namespace djj = dynamicsJRLJapan;
#include
<sot-core/exception-dynamic.h>
#include
<sot-core/matrix-homogeneous.h>
/* STD */
#include
<string>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
...
...
@@ -234,6 +236,103 @@ class SOTDYNAMIC_EXPORT Dynamic
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
);
public:
/// \name Construction of a robot by commands
///@{
///
/// \brief Create an empty device
void
createRobot
();
/// \brief create a joint
/// \param inJointName name of the joint,
/// \param inJointType type of joint in ["freeflyer","rotation","translation","anchor"],
/// \param inPosition position of the joint (4x4 homogeneous matrix).
///
/// \note joints are stored in a map with names as keys for retrieval by other
/// commands. An empty CjrlBody is also created and attached to the joint.
void
createJoint
(
const
std
::
string
&
inJointName
,
const
std
::
string
&
inJointType
,
const
ml
::
Matrix
&
inPosition
);
/// \brief Set a joint as root joint of the robot.
void
setRootJoint
(
const
std
::
string
&
inJointName
);
/// \brief Add a child joint to a joint.
/// \param inParentName name of the joint to which a child is added.
/// \param inChildName name of the child joint added.
void
addJoint
(
const
std
::
string
&
inParentName
,
const
std
::
string
&
inChildName
);
/// \brief Set bound of degree of freedom
///
/// \param inJointName name of the joint,
/// \param inDofId id of the degree of freedom in the joint,
/// \param inMinValue, inMaxValue values of degree of freedom bounds
void
setDofBounds
(
const
std
::
string
&
inJointName
,
unsigned
int
inDofId
,
double
inMinValue
,
double
inMaxValue
);
/// \brief Set mass of a body
///
/// \param inJointName name of the joint to which the body is attached,
/// \param inMass mass of the body
void
setMass
(
const
std
::
string
&
inJointName
,
double
inMass
);
/// \brief Set local center of mass of a body
///
/// \param inJointName name of the joint to which the body is attached,
/// \param inCom local center of mass.
void
setLocalCenterOfMass
(
const
std
::
string
&
inJointName
,
ml
::
Vector
inCom
);
/// \brief Set inertia matrix of a body
///
/// \param inJointName name of the joint to which the body is attached,
/// \param inMatrix inertia matrix.
void
setInertiaMatrix
(
const
std
::
string
&
inJointName
,
ml
::
Matrix
inMatrix
);
/// \brief Set specific joints
///
/// \param inJointName name of the joint,
/// \param inJointType type of joint in ['chest','waist','left-wrist','right-wrist','left-ankle','right-ankle','gaze'].
void
setSpecificJoint
(
const
std
::
string
&
inJointName
,
const
std
::
string
&
inJointType
);
/// \brief Set hand parameters
///
/// \param inRight true if right hand, false if left hand,
/// \param inCenter center of the hand in wrist local frame,
/// \param inThumbAxis thumb axis in wrist local frame,
/// \param inForefingerAxis forefinger axis in wrist local frame,
/// \param inPalmNormalAxis palm normal in wrist local frame,
void
setHandParameters
(
bool
inRight
,
const
ml
::
Vector
&
inCenter
,
const
ml
::
Vector
&
inThumbAxis
,
const
ml
::
Vector
&
inForefingerAxis
,
const
ml
::
Vector
&
inPalmNormal
);
/// \brief Set foot parameters
///
/// \param inRight true if right foot, false if left foot,
/// \param inSoleLength sole length,
/// \param inSoleWidth sole width,
/// \param inAnklePosition ankle position in foot local frame,
void
setFootParameters
(
bool
inRight
,
const
double
&
inSoleLength
,
const
double
&
inSoleWidth
,
const
ml
::
Vector
&
inAnklePosition
);
/// \brief Set gaze parameters
///
/// \param inGazeOrigin origin of the gaze in gaze joint local frame,
/// \param inGazeDirection direction of the gase in gaze joint local frame.
void
setGazeParameters
(
const
ml
::
Vector
&
inGazeOrigin
,
const
ml
::
Vector
&
inGazeDirection
);
/// @}
///
private:
/// \brief map of joints in construction.
std
::
map
<
std
::
string
,
CjrlJoint
*>
jointMap_
;
djj
::
ObjectFactory
factory_
;
};
...
...
@@ -243,4 +342,3 @@ class SOTDYNAMIC_EXPORT Dynamic
#endif // #ifndef __SOT_DYNAMIC_H__
src/dynamic-command.h
View file @
de07e548
...
...
@@ -182,6 +182,296 @@ namespace sot {
}
};
// class GetProperty
// Command CreateRobot
class
CreateRobot
:
public
Command
{
public:
virtual
~
CreateRobot
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
CreateRobot
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
std
::
vector
<
Value
::
Type
>
(),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
robot
.
createRobot
();
return
Value
();
}
};
// class CreateRobot
// Command CreateJoint
class
CreateJoint
:
public
Command
{
public:
virtual
~
CreateJoint
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
CreateJoint
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
STRING
)(
Value
::
STRING
)
(
Value
::
MATRIX
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
std
::
string
jointName
=
values
[
0
].
value
();
std
::
string
jointType
=
values
[
1
].
value
();
maal
::
boost
::
Matrix
position
=
values
[
2
].
value
();
robot
.
createJoint
(
jointName
,
jointType
,
position
);
return
Value
();
}
};
// class CreateJoint
// Command SetRootJoint
class
SetRootJoint
:
public
Command
{
public:
virtual
~
SetRootJoint
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetRootJoint
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
STRING
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
std
::
string
jointName
=
values
[
0
].
value
();
robot
.
setRootJoint
(
jointName
);
return
Value
();
}
};
// class SetRootJoint
// Command AddJoint
class
AddJoint
:
public
Command
{
public:
virtual
~
AddJoint
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
AddJoint
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
STRING
)(
Value
::
STRING
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
std
::
string
parentName
=
values
[
0
].
value
();
std
::
string
childName
=
values
[
1
].
value
();
robot
.
addJoint
(
parentName
,
childName
);
return
Value
();
}
};
// class AddJoint
// Command SetDofBounds
class
SetDofBounds
:
public
Command
{
public:
virtual
~
SetDofBounds
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetDofBounds
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
STRING
)(
Value
::
UNSIGNED
)
(
Value
::
DOUBLE
)(
Value
::
DOUBLE
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
std
::
string
jointName
=
values
[
0
].
value
();
unsigned
int
dofId
=
values
[
1
].
value
();
double
minValue
=
values
[
2
].
value
();
double
maxValue
=
values
[
3
].
value
();
robot
.
setDofBounds
(
jointName
,
dofId
,
minValue
,
maxValue
);
return
Value
();
}
};
// class SetDofBounds
// Command SetMass
class
SetMass
:
public
Command
{
public:
virtual
~
SetMass
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetMass
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
STRING
)(
Value
::
DOUBLE
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
std
::
string
jointName
=
values
[
0
].
value
();
double
mass
=
values
[
1
].
value
();
robot
.
setMass
(
jointName
,
mass
);
return
Value
();
}
};
// class SetMass
// Command SetLocalCenterOfMass
class
SetLocalCenterOfMass
:
public
Command
{
public:
virtual
~
SetLocalCenterOfMass
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetLocalCenterOfMass
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
STRING
)(
Value
::
VECTOR
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
std
::
string
jointName
=
values
[
0
].
value
();
ml
::
Vector
com
=
values
[
1
].
value
();
robot
.
setLocalCenterOfMass
(
jointName
,
com
);
return
Value
();
}
};
// class SetLocalCenterOfMass
// Command SetInertiaMatrix
class
SetInertiaMatrix
:
public
Command
{
public:
virtual
~
SetInertiaMatrix
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetInertiaMatrix
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
STRING
)(
Value
::
MATRIX
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
std
::
string
jointName
=
values
[
0
].
value
();
ml
::
Matrix
inertiaMatrix
=
values
[
1
].
value
();
robot
.
setInertiaMatrix
(
jointName
,
inertiaMatrix
);
return
Value
();
}
};
// class SetInertiaMatrix
// Command SetSpecificJoint
class
SetSpecificJoint
:
public
Command
{
public:
virtual
~
SetSpecificJoint
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetSpecificJoint
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
STRING
)(
Value
::
STRING
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
std
::
string
jointName
=
values
[
0
].
value
();
std
::
string
jointType
=
values
[
1
].
value
();
robot
.
setSpecificJoint
(
jointName
,
jointType
);
return
Value
();
}
};
// class SetSpecificJoint
// Command SetHandParameters
class
SetHandParameters
:
public
Command
{
public:
virtual
~
SetHandParameters
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetHandParameters
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
BOOL
)(
Value
::
VECTOR
)
(
Value
::
VECTOR
)(
Value
::
VECTOR
)(
Value
::
VECTOR
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
bool
right
=
values
[
0
].
value
();
ml
::
Vector
center
=
values
[
1
].
value
();
ml
::
Vector
thumbAxis
=
values
[
2
].
value
();
ml
::
Vector
forefingerAxis
=
values
[
3
].
value
();
ml
::
Vector
palmNormalAxis
=
values
[
4
].
value
();
robot
.
setHandParameters
(
right
,
center
,
thumbAxis
,
forefingerAxis
,
palmNormalAxis
);
return
Value
();
}
};
// class SetHandParameters
// Command SetFootParameters
class
SetFootParameters
:
public
Command
{
public:
virtual
~
SetFootParameters
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetFootParameters
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
BOOL
)(
Value
::
DOUBLE
)
(
Value
::
DOUBLE
)(
Value
::
VECTOR
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
bool
right
=
values
[
0
].
value
();
double
soleLength
=
values
[
1
].
value
();
double
soleWidth
=
values
[
2
].
value
();
ml
::
Vector
anklePosition
=
values
[
3
].
value
();
robot
.
setFootParameters
(
right
,
soleLength
,
soleWidth
,
anklePosition
);
return
Value
();
}
};
// class Setfootparameters
// Command SetGazeParameters
class
SetGazeParameters
:
public
Command
{
public:
virtual
~
SetGazeParameters
()
{}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetGazeParameters
(
Dynamic
&
entity
,
const
std
::
string
&
docstring
)
:
Command
(
entity
,
boost
::
assign
::
list_of
(
Value
::
VECTOR
)(
Value
::
VECTOR
),
docstring
)
{
}
virtual
Value
doExecute
()
{
Dynamic
&
robot
=
static_cast
<
Dynamic
&>
(
owner
());
std
::
vector
<
Value
>
values
=
getParameterValues
();
ml
::
Vector
gazeOrigin
=
values
[
0
].
value
();
ml
::
Vector
gazeDirection
=
values
[
1
].
value
();
robot
.
setGazeParameters
(
gazeOrigin
,
gazeDirection
);
return
Value
();
}
};
// class SetGazeParameters
}
// namespace command
}
//namespace sot
...
...
src/dynamic.cpp
View file @
de07e548
...
...
@@ -37,6 +37,37 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Dynamic,"Dynamic");
using
namespace
std
;
static
matrix4d
maalToMatrix4d
(
const
ml
::
Matrix
&
inMatrix
)
{
matrix4d
homogeneous
;
for
(
unsigned
int
r
=
0
;
r
<
4
;
r
++
)
{
for
(
unsigned
int
c
=
0
;
c
<
4
;
c
++
)
{
homogeneous
(
r
,
c
)
=
inMatrix
(
r
,
c
);
}
}
return
homogeneous
;
}
static
vector3d
maalToVector3d
(
const
ml
::
Vector
&
inVector
)
{
vector3d
vector
;
vector
(
0
)
=
inVector
(
0
);
vector
(
1
)
=
inVector
(
1
);
vector
(
2
)
=
inVector
(
2
);
return
vector
;
}
static
matrix3d
maalToMatrix3d
(
const
ml
::
Matrix
&
inMatrix
)
{
matrix3d
matrix
;
for
(
unsigned
int
r
=
0
;
r
<
3
;
r
++
)
{
for
(
unsigned
int
c
=
0
;
c
<
3
;
c
++
)
{
matrix
(
r
,
c
)
=
inMatrix
(
r
,
c
);
}
}
return
matrix
;
}
Dynamic
::
Dynamic
(
const
std
::
string
&
name
,
bool
build
)
:
Entity
(
name
)
...
...
@@ -162,8 +193,139 @@ Dynamic( const std::string & name, bool build )
" - a string: name of the property,
\n
"
" - a string: value of the property.
\n
"
"
\n
"
;
addCommand
(
"setProperty"
,
new
command
::
SetProperty
(
*
this
,
docstring
));
addCommand
(
"setProperty"
,
new
command
::
SetProperty
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Get a property
\n
"
"
\n
"
" Input:
\n
"
" - a string: name of the property,
\n
"
" Return:
\n
"
" - a string: value of the property
\n
"
;
addCommand
(
"getProperty"
,
new
command
::
GetProperty
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Create an empty robot
\n
"
"
\n
"
;
addCommand
(
"createRobot"
,
new
command
::
CreateRobot
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Create a joint
\n
"
"
\n
"
" Input:
\n
"
" - a string: name of the joint,
\n
"
" - a string: type of the joint in ['freeflyer', 'rotation',
\n
"
"' translation', 'anchor'],
\n
"
" - a matrix: (homogeneous) position of the joint.
\n
"
"
\n
"
;
addCommand
(
"createJoint"
,
new
command
::
CreateJoint
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set the root joint of the robot
\n
"
"
\n
"
" Input:
\n
"
" - a string: name of the joint.
\n
"
"
\n
"
;
addCommand
(
"setRootJoint"
,
new
command
::
SetRootJoint
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Add a child joint to a joint
\n
"
"
\n
"
" Input:
\n
"
" - a string: name of the parent joint,
\n
"
" - a string: name of the child joint.
\n
"
"
\n
"
;
addCommand
(
"addJoint"
,
new
command
::
AddJoint
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set the bounds of a joint degree of freedom
\n
"
"
\n
"
" Input:
\n
"
" - a string: the name of the joint,
\n
"
" - a non-negative integer: the dof id in the joint
\n
"
" - a floating point number: the minimal value,
\n
"
" - a floating point number: the maximal value.
\n
"
"
\n
"
;
addCommand
(
"setDofBounds"
,
new
command
::
SetDofBounds
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set the mass of the body attached to a joint
\n
"
"
\n
"
" Input:
\n
"
" - a string: name of the joint,
\n
"
" - a floating point number: the mass of the body.
\n
"
"
\n
"
;
addCommand
(
"setMass"
,
new
command
::
SetMass
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set the position of the center of mass of a body
\n
"
"
\n
"
" Input:
\n
"
" - a string: name of the joint,
\n
"
" - a vector: the coordinate of the center of mass in the local
\n
"
" frame of the joint.
\n
"
"
\n
"
;
addCommand
(
"setLocalCenterOfMass"
,
new
command
::
SetLocalCenterOfMass
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set inertia matrix of a body attached to a joint
\n
"
"
\n
"
" Input:
\n
"
" - a string: name of the joint,
\n
"
" - a matrix: inertia matrix.
\n
"
"
\n
"
;
addCommand
(
"setInertiaMatrix"
,
new
command
::
SetInertiaMatrix
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set specific joints of humanoid robot
\n
"
"
\n
"
" Input:
\n
"
" - a string: name of the joint,
\n
"
" - a string: type of the joint in ['waist', 'chest',"
" 'left-wrist',
\n
"
" 'right-wrist', 'left-ankle', 'right-ankle',"
" 'gaze']
\n
"
"
\n
"
;
addCommand
(
"setSpecificJoint"
,
new
command
::
SetSpecificJoint
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set hand parameters
\n
"
"
\n
"
" Input:
\n
"
" - a boolean: whether right hand or not,
\n
"
" - a vector: the center of the hand in the wrist local frame,
\n
"
" - a vector: the thumb axis in the wrist local frame,
\n
"
" - a vector: the forefinger axis in the wrist local frame,
\n
"
" - a vector: the palm normal in the wrist local frame.
\n
"
"
\n
"
;
addCommand
(
"setHandParameters"
,
new
command
::
SetHandParameters
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set foot parameters
\n
"
"
\n
"
" Input:
\n
"
" - a boolean: whether right hand or not,
\n
"
" - a floating point number: the sole length,
\n
"
" - a floating point number: the sole width,
\n
"
" - a vector: the position of the ankle in the foot local frame.
\n
"
"
\n
"
;
addCommand
(
"setFootParameters"
,
new
command
::
SetFootParameters
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Set parameters of the gaze
\n
"
"
\n
"
" Input:
\n
"
" - a vector: the gaze origin,
\n
"
" - a vector: the gaze direction.
\n
"
"
\n
"
;
addCommand
(
"setGazeParameters"
,
new
command
::
SetGazeParameters
(
*
this
,
docstring
));
sotDEBUGOUT
(
5
);
}
...
...
@@ -1124,5 +1286,252 @@ commandLine( const std::string& cmdLine,
}