From de07e5484da553e94c7e6c863cc62e1cbdef8702 Mon Sep 17 00:00:00 2001
From: florent <florent@laas.fr>
Date: Sat, 25 Dec 2010 22:18:00 +0100
Subject: [PATCH] Add commands to build a robot
* include/sot-dynamic/dynamic.h,
* src/dynamic-command.h,
* src/dynamic.cpp.
---
include/sot-dynamic/dynamic.h | 106 ++++++++-
src/dynamic-command.h | 290 ++++++++++++++++++++++++
src/dynamic.cpp | 413 +++++++++++++++++++++++++++++++++-
3 files changed, 803 insertions(+), 6 deletions(-)
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index c9ca73a..fcef9a8 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -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__
-
diff --git a/src/dynamic-command.h b/src/dynamic-command.h
index 2b11d56..c322754 100644
--- a/src/dynamic-command.h
+++ b/src/dynamic-command.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
diff --git a/src/dynamic.cpp b/src/dynamic.cpp
index 932ea64..c234182 100644
--- a/src/dynamic.cpp
+++ b/src/dynamic.cpp
@@ -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()
+{
+ if (m_HDR)
+ delete m_HDR;
+ m_HDR = factory_.createHumanoidDynamicRobot();
+}
+
+void Dynamic::createJoint(const std::string& inJointName,
+ const std::string& inJointType,
+ const ml::Matrix& inPosition)
+{
+ if (jointMap_.count(inJointName) == 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "a joint with name " + inJointName +
+ " has already been created.");
+ }
+ matrix4d position = maalToMatrix4d(inPosition);
+ CjrlJoint* joint=NULL;
+
+ if (inJointType == "freeflyer") {
+ joint = factory_.createJointFreeflyer(position);
+ } else if (inJointType == "rotation") {
+ joint = factory_.createJointRotation(position);
+ } else if (inJointType == "translation") {
+ joint = factory_.createJointTranslation(position);
+ } else if (inJointType == "anchor") {
+ joint = factory_.createJointAnchor(position);
+ } else {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ inJointType + " is not a valid type.\n"
+ "Valid types are 'freeflyer', 'rotation', 'translation', 'anchor'.");
+ }
+ jointMap_[inJointName] = joint;
+}
+
+void Dynamic::setRootJoint(const std::string& inJointName)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ if (jointMap_.count(inJointName) != 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "No joint with name " + inJointName +
+ " has been created.");
+ }
+ m_HDR->rootJoint(*jointMap_[inJointName]);
+}
+
+void Dynamic::addJoint(const std::string& inParentName,
+ const std::string& inChildName)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ if (jointMap_.count(inParentName) != 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "No joint with name " + inParentName +
+ " has been created.");
+ }
+ if (jointMap_.count(inChildName) != 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "No joint with name " + inChildName +
+ " has been created.");
+ }
+ jointMap_[inParentName]->addChildJoint(*(jointMap_[inChildName]));
+}
+
+void Dynamic::setDofBounds(const std::string& inJointName,
+ unsigned int inDofId,
+ double inMinValue, double inMaxValue)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ if (jointMap_.count(inJointName) != 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "No joint with name " + inJointName +
+ " has been created.");
+ }
+ jointMap_[inJointName]->lowerBound(inDofId, inMinValue);
+ jointMap_[inJointName]->upperBound(inDofId, inMaxValue);
+}
+
+void Dynamic::setMass(const std::string& inJointName, double inMass)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ if (jointMap_.count(inJointName) != 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "No joint with name " + inJointName +
+ " has been created.");
+ }
+ CjrlJoint* joint = jointMap_[inJointName];
+ if (!joint->linkedBody()) {
+ joint->setLinkedBody(*(factory_.createBody()));
+ }
+ CjrlBody& body = *(joint->linkedBody());
+ body.mass(inMass);
+}
+
+void Dynamic::setLocalCenterOfMass(const std::string& inJointName,
+ ml::Vector inCom)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ if (jointMap_.count(inJointName) != 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "No joint with name " + inJointName +
+ " has been created.");
+ }
+ CjrlJoint* joint = jointMap_[inJointName];
+ if (!joint->linkedBody()) {
+ joint->setLinkedBody(*(factory_.createBody()));
+ }
+ CjrlBody& body = *(joint->linkedBody());
+ body.localCenterOfMass(maalToVector3d(inCom));
+}
+
+void Dynamic::setInertiaMatrix(const std::string& inJointName,
+ ml::Matrix inMatrix)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ if (jointMap_.count(inJointName) != 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "No joint with name " + inJointName +
+ " has been created.");
+ }
+ CjrlJoint* joint = jointMap_[inJointName];
+ if (!joint->linkedBody()) {
+ joint->setLinkedBody(*(factory_.createBody()));
+ }
+ CjrlBody& body = *(joint->linkedBody());
+ body.inertiaMatrix(maalToMatrix3d(inMatrix));
+}
+void Dynamic::setSpecificJoint(const std::string& inJointName,
+ const std::string& inJointType)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ if (jointMap_.count(inJointName) != 1) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "No joint with name " + inJointName +
+ " has been created.");
+ }
+ CjrlJoint* joint = jointMap_[inJointName];
+ if (inJointType == "chest") {
+ } else if (inJointType == "waist") {
+ m_HDR->waist(joint);
+ } else if (inJointType == "chest") {
+ m_HDR->chest(joint);
+ } else if (inJointType == "left-wrist") {
+ m_HDR->leftWrist(joint);
+ // Create hand
+ CjrlHand* hand = factory_.createHand(joint);
+ m_HDR->leftHand(hand);
+ } else if (inJointType == "right-wrist") {
+ m_HDR->rightWrist(joint);
+ // Create hand
+ CjrlHand* hand = factory_.createHand(joint);
+ m_HDR->rightHand(hand);
+ } else if (inJointType == "left-ankle") {
+ m_HDR->leftAnkle(joint);
+ // Create foot
+ CjrlFoot* foot = factory_.createFoot(joint);
+ m_HDR->leftFoot(foot);
+ } else if (inJointType == "right-ankle") {
+ m_HDR->rightAnkle(joint);
+ // Create foot
+ CjrlFoot* foot = factory_.createFoot(joint);
+ m_HDR->rightFoot(foot);
+ } else if (inJointType == "gaze") {
+ m_HDR->gazeJoint(joint);
+ } else {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ inJointType + " is not a valid type.\n"
+ "Valid types are 'waist', 'chest', 'left-wrist',\n'right-wrist', 'left-ankle', 'right-ankle', 'gaze'.");
+ }
+}
+void Dynamic::setHandParameters(bool inRight, const ml::Vector& inCenter,
+ const ml::Vector& inThumbAxis,
+ const ml::Vector& inForefingerAxis,
+ const ml::Vector& inPalmNormal)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ CjrlHand *hand = NULL;
+ if (inRight) {
+ hand = m_HDR->rightHand();
+ } else {
+ hand = m_HDR->leftHand();
+ }
+ if (!hand) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "hand has not been defined yet");
+ }
+ hand->setCenter(maalToVector3d(inCenter));
+ hand->setThumbAxis(maalToVector3d(inThumbAxis));
+ hand->setForeFingerAxis(maalToVector3d(inForefingerAxis));
+ hand->setPalmNormal(maalToVector3d(inPalmNormal));
+}
+
+void Dynamic::setFootParameters(bool inRight, const double& inSoleLength,
+ const double& inSoleWidth,
+ const ml::Vector& inAnklePosition)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ CjrlFoot *foot = NULL;
+ if (inRight) {
+ foot = m_HDR->rightFoot();
+ } else {
+ foot = m_HDR->leftFoot();
+ }
+ if (!foot) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "foot has not been defined yet");
+ }
+ foot->setSoleSize(inSoleLength, inSoleWidth);
+ foot->setAnklePositionInLocalFrame(maalToVector3d(inAnklePosition));
+}
+
+void Dynamic::setGazeParameters(const ml::Vector& inGazeOrigin,
+ const ml::Vector& inGazeDirection)
+{
+ if (!m_HDR) {
+ SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
+ "you must create a robot first.");
+ }
+ m_HDR->gaze(maalToVector3d(inGazeOrigin),
+ maalToVector3d(inGazeDirection));
+}
--
GitLab