diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index f3629b8828746d7c3b6fecc4f79fc2f03ec8dac7..ed19bb81c4eb35bc323aa38807b2560e29bb90a2 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -339,7 +339,8 @@ class SOTDYNAMIC_EXPORT Dynamic
};
-
+ std::ostream& operator<<(std::ostream& os, const CjrlHumanoidDynamicRobot& r);
+ std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
} // namespace sot
diff --git a/src/dynamic-command.h b/src/dynamic-command.h
index a28d5ebea4e2ee01dbe371c0a6fbbf4d17285709..4b732616562f129dbc169700bc25ac42fc602120 100644
--- a/src/dynamic-command.h
+++ b/src/dynamic-command.h
@@ -20,6 +20,7 @@
#ifndef DYNAMIC_COMMAND_H
#define DYNAMIC_COMMAND_H
+ #include <fstream>
#include <boost/assign/list_of.hpp>
#include <dynamic-graph/command.h>
@@ -493,6 +494,52 @@ namespace sot {
return Value();
}
}; // class InitializeRobot
+
+ // Command GetDimension
+ class GetDimension : public Command
+ {
+ public:
+ virtual ~GetDimension() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ GetDimension(Dynamic& entity, const std::string& docstring) :
+ Command(entity, std::vector<Value::Type>(),
+ docstring)
+ {
+ }
+ virtual Value doExecute()
+ {
+ Dynamic& robot = static_cast<Dynamic&>(owner());
+ unsigned int dimension = robot.m_HDR->numberDof();
+ return Value(dimension);
+ }
+ }; // class GetDimension
+
+ // Command Write
+ class Write : public Command
+ {
+ public:
+ virtual ~Write() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Write(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 filename = values[0].value();
+ std::ofstream file(filename.c_str(), std::ios_base::out);
+ file << *(robot.m_HDR);
+ file.close();
+ return Value();
+ }
+ }; // class Write
} // namespace command
} //namespace sot
diff --git a/src/dynamic.cpp b/src/dynamic.cpp
index 7e3fee394ad595cb48aba4c41c245719707020e9..5cae3eeecd121899975e6ee8404f15c0d99d7cd8 100644
--- a/src/dynamic.cpp
+++ b/src/dynamic.cpp
@@ -333,6 +333,24 @@ Dynamic( const std::string & name, bool build )
addCommand("initializeRobot",
new command::InitializeRobot(*this, docstring));
+ docstring = " \n"
+ " Get the dimension of the robot configuration.\n"
+ " \n";
+ " Return:\n";
+ " an unsigned int: the dimension.\n";
+ " \n";
+ addCommand("getDimension",
+ new command::GetDimension(*this, docstring));
+
+ docstring = " \n"
+ " Write the robot kinematic chain in a file.\n"
+ " \n";
+ " Input:\n";
+ " a string: a filename.\n";
+ " \n";
+ addCommand("write",
+ new command::Write(*this, docstring));
+
sotDEBUGOUT(5);
}
@@ -1539,6 +1557,115 @@ void Dynamic::setGazeParameters(const ml::Vector& inGazeOrigin,
SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
"you must create a robot first.");
}
- m_HDR->gaze(maalToVector3d(inGazeOrigin),
- maalToVector3d(inGazeDirection));
+ m_HDR->gaze(maalToVector3d(inGazeDirection),
+ maalToVector3d(inGazeOrigin));
+}
+
+std::ostream& sot::operator<<(std::ostream& os,
+ const CjrlHumanoidDynamicRobot& robot)
+{
+ os << "Device: " << &robot << std::endl;
+ os << std::endl;
+ os << " gaze: " << robot.gazeJoint() << std::endl;
+ os << " waist: " << robot.waist() << std::endl;
+ os << " chest: " << robot.waist() << std::endl;
+ os << " left wrist: " << robot.leftWrist() << std::endl;
+ os << " right wrist: " << robot.rightWrist() << std::endl;
+ os << " left ankle: " << robot.leftAnkle() << std::endl;
+ os << " right ankle: " << robot.rightAnkle() << std::endl;
+ os << std::endl;
+ os << " gaze origin: " << robot.gazeOrigin() << std::endl;
+ os << " gaze direction: " << robot.gazeDirection() << std::endl;
+ os << std::endl;
+ os << " left hand" << std::endl;
+ CjrlHand* hand = robot.leftHand();
+ vector3d v;
+ hand-> getCenter(v);
+ os << " center: " << v << std::endl;
+ hand-> getThumbAxis(v);
+ os << " thumb axis: " << v << std::endl;
+ hand-> getForeFingerAxis(v);
+ os << " forefinger axis: " << v << std::endl;
+ hand-> getPalmNormal(v);
+ os << " palm normal: " << v << std::endl;
+ os << " right hand" << std::endl;
+ hand = robot.rightHand();
+ hand-> getCenter(v);
+ os << " center: " << v << std::endl;
+ hand-> getThumbAxis(v);
+ os << " thumb axis: " << v << std::endl;
+ hand-> getForeFingerAxis(v);
+ os << " forefinger axis: " << v << std::endl;
+ hand-> getPalmNormal(v);
+ os << " palm normal: " << v << std::endl;
+ os << " left foot" << std::endl;
+ CjrlFoot* foot = robot.leftFoot();
+ double length, width;
+ foot->getSoleSize(length, width);
+ foot->getAnklePositionInLocalFrame(v);
+ os << " sole length: " << length << std::endl;
+ os << " sole width: " << width << std::endl;
+ os << " ankle position in foot frame: " << v << std::endl;
+ os << " right foot" << std::endl;
+ foot = robot.rightFoot();
+ foot->getSoleSize(length, width);
+ foot->getAnklePositionInLocalFrame(v);
+ os << " sole length: " << length << std::endl;
+ os << " sole width: " << width << std::endl;
+ os << " ankle position in foot frame: " << v << std::endl;
+ os << std::endl;
+ os << " Current configuration: " << robot.currentConfiguration()
+ << std::endl;
+ os << std::endl;
+ os << std::endl;
+ os << " Writing kinematic chain" << std::endl;
+
+ //
+ // Go through joints and output each joint
+ //
+ CjrlJoint* joint = robot.rootJoint();
+
+ if (joint) {
+ os << *joint << std::endl;
+ }
+ // Get position of center of mass
+ MAL_S3_VECTOR(com, double);
+ com = robot.positionCenterOfMass();
+
+ //debug
+ os <<"total mass "<<robot.mass() <<", COM: "
+ << MAL_S3_VECTOR_ACCESS(com, 0)
+ <<", "<< MAL_S3_VECTOR_ACCESS(com, 1)
+ <<", "<< MAL_S3_VECTOR_ACCESS(com, 2)
+ <<std::endl;
+ return os;
+}
+
+std::ostream& sot::operator<<(std::ostream& os, const CjrlJoint& joint)
+{
+ os << "CjrlJoint:" << &joint << std::endl;
+ os << "Rank in configuration "
+ << joint.rankInConfiguration() << std::endl;
+ os << "Current transformation:" << std::endl;
+ os << joint.currentTransformation() << std:: endl;
+ os << std::endl;
+
+ CjrlBody* body = joint.linkedBody();
+ if (body) {
+ os << "Attached body:" << std::endl;
+ os << "Mass of the attached body: " << body->mass() << std::endl;
+ os << "Local center of mass:" << body->localCenterOfMass() << std::endl;
+ os << "Inertia matrix:" << std::endl;
+ os << body->inertiaMatrix() << std::endl;
+ } else {
+ os << "No attached body" << std::endl;
+ }
+
+ for (unsigned int iChild=0; iChild < joint.countChildJoints();
+ iChild++) {
+ os << *(joint.childJoint(iChild)) << std::endl;
+ os <<std::endl;
+ }
+
+ return os;
}