Commit 8fe434c2 authored by florent's avatar florent
Browse files

Add a command to write robot in a file (and one to get robot dimension).

    * include/sot-dynamic/dynamic.h,
    * src/dynamic-command.h,
    * src/dynamic.cpp.
parent d4a2fbea
...@@ -339,7 +339,8 @@ class SOTDYNAMIC_EXPORT Dynamic ...@@ -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 } // namespace sot
......
...@@ -20,6 +20,7 @@ ...@@ -20,6 +20,7 @@
#ifndef DYNAMIC_COMMAND_H #ifndef DYNAMIC_COMMAND_H
#define DYNAMIC_COMMAND_H #define DYNAMIC_COMMAND_H
#include <fstream>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
#include <dynamic-graph/command.h> #include <dynamic-graph/command.h>
...@@ -493,6 +494,52 @@ namespace sot { ...@@ -493,6 +494,52 @@ namespace sot {
return Value(); return Value();
} }
}; // class InitializeRobot }; // 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 command
} //namespace sot } //namespace sot
......
...@@ -333,6 +333,24 @@ Dynamic( const std::string & name, bool build ) ...@@ -333,6 +333,24 @@ Dynamic( const std::string & name, bool build )
addCommand("initializeRobot", addCommand("initializeRobot",
new command::InitializeRobot(*this, docstring)); 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); sotDEBUGOUT(5);
} }
...@@ -1539,6 +1557,115 @@ void Dynamic::setGazeParameters(const ml::Vector& inGazeOrigin, ...@@ -1539,6 +1557,115 @@ void Dynamic::setGazeParameters(const ml::Vector& inGazeOrigin,
SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL, SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
"you must create a robot first."); "you must create a robot first.");
} }
m_HDR->gaze(maalToVector3d(inGazeOrigin), m_HDR->gaze(maalToVector3d(inGazeDirection),
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;
} }
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment