Commit 56a93b82 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

Added three commands to create jacobian and position signals alone.

parent f348dbc7
......@@ -238,6 +238,10 @@ class SOTDYNAMIC_EXPORT Dynamic
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
void cmd_createOpPointSignals ( const std::string& sig,const std::string& j );
void cmd_createJacobianWorldSignal ( const std::string& sig,const std::string& j );
void cmd_createJacobianEndEffectorSignal( const std::string& sig,const std::string& j );
void cmd_createPositionSignal ( const std::string& sig,const std::string& j );
public:
/// \name Construction of a robot by commands
......@@ -352,6 +356,8 @@ class SOTDYNAMIC_EXPORT Dynamic
/// \brief map of joints in construction.
std::map<std::string, CjrlJoint*> jointMap_;
djj::ObjectFactory factory_;
/// Return a specific joint, being given a name by string inside a short list.
CjrlJoint* getJointByName( const std::string& jointName );
};
......
......@@ -84,55 +84,6 @@ namespace sot {
}
}; // class Parse
// Command CreateOpPoint
class CreateOpPoint : public Command
{
public:
virtual ~CreateOpPoint() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
CreateOpPoint(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 opPointName = values[0].value();
std::string jointName = values[1].value();
CjrlJoint* joint = NULL;
if (jointName == "gaze") {
joint = robot.m_HDR->gazeJoint();
} else if (jointName == "left-ankle") {
joint = robot.m_HDR->leftAnkle();
} else if (jointName == "right-ankle") {
joint = robot.m_HDR->rightAnkle();
} else if (jointName == "left-wrist") {
joint = robot.m_HDR->leftWrist();
} else if (jointName == "right-wrist") {
joint = robot.m_HDR->rightWrist();
} else if (jointName == "waist") {
joint = robot.m_HDR->waist();
} else if (jointName == "chest") {
joint = robot.m_HDR->chest();
} else if (jointName == "gaze") {
joint = robot.m_HDR->gazeJoint();
} else {
throw ExceptionDynamic(ExceptionDynamic::GENERIC,
jointName + " is not a valid name."
" Valid names are \n"
"gaze, left-ankle, right-ankle, left-wrist,"
" right-wrist, waist, chest.");
}
robot.createEndeffJacobianSignal(std::string("J")+opPointName, joint);
robot.createPositionSignal(opPointName, joint);
return Value();
}
}; // class CreateOpPoint
// Command SetProperty
class SetProperty : public Command
{
......
......@@ -18,21 +18,22 @@
* with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
*/
#include <sot-core/debug.h>
#include <sot-dynamic/dynamic.h>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <jrl/mal/matrixabstractlayer.hh>
#include <sot-dynamic/dynamic.h>
#include <sot-core/debug.h>
#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include "../src/dynamic-command.h"
using namespace sot;
using namespace dynamicgraph;
......@@ -178,16 +179,38 @@ Dynamic( const std::string & name, bool build )
addCommand("parse",
new command::Parse(*this, docstring));
// CreateOpPoint
docstring = " \n"
" Create an operational point attached to a robot joint local frame.\n"
" \n"
" Input: \n"
" - a string: name of the operational point,\n"
" - a string: name the joint, among (gaze, left-ankle, right ankle\n"
" , left-wrist, right-wrist, waist, chest).\n"
"\n";
addCommand("createOpPoint", new command::CreateOpPoint(*this, docstring));
{
using namespace ::dynamicgraph::command;
// CreateOpPoint
docstring = " \n"
" Create an operational point attached to a robot joint local frame.\n"
" \n"
" Input: \n"
" - a string: name of the operational point,\n"
" - a string: name the joint, among (gaze, left-ankle, right ankle\n"
" , left-wrist, right-wrist, waist, chest).\n"
"\n";
addCommand("createOpPoint",
makeCommandVoid2(*this,&Dynamic::cmd_createOpPointSignals,
docstring));
docstring = docCommandVoid2("Create a jacobian (world frame) signal only for one joint.",
"string (signal name)","string (joint name)");
addCommand("createJacobian",
makeCommandVoid2(*this,&Dynamic::cmd_createJacobianWorldSignal,
docstring));
docstring = docCommandVoid2("Create a jacobian (endeff frame) signal only for one joint.",
"string (signal name)","string (joint name)");
addCommand("createJacobianEndEff",
makeCommandVoid2(*this,&Dynamic::cmd_createJacobianEndEffectorSignal,
docstring));
docstring = docCommandVoid2("Create a position (matrix homo) signal only for one joint.",
"string (signal name)","string (joint name)");
addCommand("createPosition",
makeCommandVoid2(*this,&Dynamic::cmd_createPositionSignal,docstring));
}
// SetProperty
docstring = " \n"
......@@ -1190,7 +1213,62 @@ getLowerJointLimits(ml::Vector& res, const int&)
return res;
}
/* --- COMMANDS ------------------------------------------------------------- */
/* --- COMMANDS ------------------------------------------------------------- */
/* --- COMMANDS ------------------------------------------------------------- */
CjrlJoint* Dynamic::getJointByName( const std::string& jointName )
{
if (jointName == "gaze") {
return m_HDR->gazeJoint();
} else if (jointName == "left-ankle") {
return m_HDR->leftAnkle();
} else if (jointName == "right-ankle") {
return m_HDR->rightAnkle();
} else if (jointName == "left-wrist") {
return m_HDR->leftWrist();
} else if (jointName == "right-wrist") {
return m_HDR->rightWrist();
} else if (jointName == "waist") {
return m_HDR->waist();
} else if (jointName == "chest") {
return m_HDR->chest();
} else if (jointName == "gaze") {
return m_HDR->gazeJoint();
} else {
throw ExceptionDynamic(ExceptionDynamic::GENERIC,
jointName + " is not a valid name."
" Valid names are \n"
"gaze, left-ankle, right-ankle, left-wrist,"
" right-wrist, waist, chest.");
}
}
void Dynamic::cmd_createOpPointSignals( const std::string& opPointName,
const std::string& jointName )
{
CjrlJoint* joint = getJointByName(jointName);
createEndeffJacobianSignal(std::string("J")+opPointName, joint);
createPositionSignal(opPointName, joint);
}
void Dynamic::cmd_createJacobianWorldSignal( const std::string& signalName,
const std::string& jointName )
{
CjrlJoint* joint = getJointByName(jointName);
createJacobianSignal(signalName, joint);
}
void Dynamic::cmd_createJacobianEndEffectorSignal( const std::string& signalName,
const std::string& jointName )
{
CjrlJoint* joint = getJointByName(jointName);
createEndeffJacobianSignal(signalName, joint);
}
void Dynamic::cmd_createPositionSignal( const std::string& signalName,
const std::string& jointName )
{
CjrlJoint* joint = getJointByName(jointName);
createPositionSignal(signalName, joint);
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
......
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