diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index a1af8798d2393ec8a734d1326b444fdf703a493b..a0dd0cae2ccdd34d6c294d26eb9d4503186ff90a 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -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 );
};
diff --git a/src/dynamic-command.h b/src/dynamic-command.h
index 91db668111c0f4e90f3735086b5653f7fefe5280..d289dfbd9a6053c2be0a4bb5bf74f49af27d8b77 100644
--- a/src/dynamic-command.h
+++ b/src/dynamic-command.h
@@ -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
{
diff --git a/src/dynamic.cpp b/src/dynamic.cpp
index 43d14402cde78334b9370d24b1d2ea755c4a03a0..1eff5573b22c1f98ad3493b95ae82a68a28f28c8 100644
--- a/src/dynamic.cpp
+++ b/src/dynamic.cpp
@@ -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 --------------------------------------------------------------- */