Commit de07e548 authored by florent's avatar florent
Browse files

Add commands to build a robot

    * include/sot-dynamic/dynamic.h,
    * src/dynamic-command.h,
    * src/dynamic.cpp.
parent 5f67c35e
......@@ -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__
......@@ -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
......
......@@ -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,
}
void Dynamic::createRobot()
{