Unverified Commit a78007e7 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub

Merge pull request #4 from jmirabel/master

Major update
parents 0fd9dbb5 308212d0
......@@ -23,13 +23,13 @@ SET(CXX_DISABLE_WERROR TRUE)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAME sot-hpp)
SET(PROJECT_NAME sot_hpp)
SET(PROJECT_DESCRIPTION "Bridge between hpp and sot")
SET(PROJECT_URL "")
SETUP_PROJECT()
FINDPYTHON(REQUIRED 2.7 EXACT)
FINDPYTHON(REQUIRED EXACT 2.7)
ADD_REQUIRED_DEPENDENCY ("dynamic_graph_bridge >= 3")
# Needs only the Python classes related to ConstraintGraphFactory.
......@@ -38,16 +38,12 @@ ADD_REQUIRED_DEPENDENCY ("sot-core >= 3")
ADD_REQUIRED_DEPENDENCY ("dynamic-graph-python >= 2")
ADD_REQUIRED_DEPENDENCY ("sot_hpp_msgs")
SET(FILES
supervisor.py
tools.py
ros_interface.py
factory.py
__init__.py)
FOREACH(F ${FILES})
PYTHON_INSTALL_ON_SITE("sot_hpp" ${F})
ENDFOREACH()
INSTALL(FILES
package.xml
DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
INSTALL(PROGRAMS
scripts/start_supervisor.py
DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME}/scripts)
ADD_SUBDIRECTORY(src)
......
<package>
<name>sot_hpp</name>
<version>0.0.0</version>
<description> SoT - HPP package. </description>
<maintainer email="jmirabel@laas.fr">Joseph Mirabel</maintainer>
<author>Joseph Mirabel</author>
<license>LGPL</license>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>sot_hpp_msgs</run_depend>
</package>
#!/usr/bin/python
import sys, getopt
def usage():
print ("Usage: " + sys.argv[0] + " --input script.py [--prefix prefix]")
sys.exit(1)
opts, args = getopt.getopt (sys.argv[1:], "i:p:", ["input=", "prefix="])
f = None
prefix = ""
for opt, arg in opts:
if opt == "-i" or opt == "--input":
from os.path import isfile
if not isfile(arg):
raise ValueError ("File not found: " + arg)
f = arg
elif opt == "-p" or opt == "--prefix":
prefix = arg
else:
usage()
if f is None:
usage()
import rospy
from std_srvs.srv import *
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
rospy.init_node ('start_supervisor')
rospy.loginfo ("Will read file: " + f)
def _runCommandPrint (ans):
if ans.result != "None":
rospy.loginfo (ans.result)
if len(ans.standardoutput) > 0:
rospy.loginfo (ans.standardoutput)
if len(ans.standarderror) > 0:
rospy.logerr (ans.standarderror)
def launchScript(code,title):
rospy.loginfo(title)
indent = ' '
indenting = False
for line in code:
if indenting:
if line == '' or line.startswith(indent):
rospy.loginfo (".. " + line)
codeblock += '\n' + line
continue
else:
answer = runCommandClient(str(codeblock))
_runCommandPrint (answer)
indenting = False
if line != '' and line[0] != '#':
rospy.loginfo (">> " + line)
if line.endswith(':'):
indenting = True
codeblock = line
else:
answer = runCommandClient(str(line))
_runCommandPrint (answer)
rospy.loginfo("...done with "+title)
def makeRosInterface():
from sot_hpp.ros_interface import RosInterface
import rospy
ri = RosInterface ()
ri.setupHppJoints (prefix = prefix)
return ri
# Waiting for services
try:
rospy.loginfo("Waiting for run_command")
rospy.wait_for_service('/run_command')
rospy.loginfo("...ok")
rospy.loginfo("Waiting for start_dynamic_graph")
rospy.wait_for_service('/start_dynamic_graph')
rospy.loginfo("...ok")
runCommandClient = rospy.ServiceProxy('/run_command', RunCommand)
runCommandStartDynamicGraph = rospy.ServiceProxy('/start_dynamic_graph', Empty)
initCode = open( f, "r").read().split("\n")
rospy.loginfo("Stack of Tasks launched")
launchScript(initCode,'initialize SoT')
ri = makeRosInterface ()
runCommandStartDynamicGraph()
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
rospy.spin()
......@@ -61,12 +61,13 @@ MACRO(DYNAMIC_GRAPH_HPP_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME)
DESTINATION ${PYTHON_INSTALL_DIR}
)
ENDMACRO(DYNAMIC_GRAPH_PYTHON_MODULE SUBMODULENAME)
ENDMACRO()
SET(LIBRARY_NAME ${PROJECT_NAME})
ADD_LIBRARY(${LIBRARY_NAME}
SHARED
holonomic-constraint.cc
event.cc
boolean-reduction.cc
ros_subscribe.cpp
......@@ -80,9 +81,20 @@ INSTALL(TARGETS ${LIBRARY_NAME}
DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
)
SET(NEW_ENTITY_CLASS "Event" "CompareDouble" "RosQueuedSubscribe")
SET(NEW_ENTITY_CLASS "Event" "CompareDouble" "RosQueuedSubscribe" "HolonomicConstraint")
DYNAMIC_GRAPH_HPP_PYTHON_MODULE("sot" ${LIBRARY_NAME} wrap)
FILE(WRITE ${CMAKE_CURRENT_BINARY_DIR}/dynamic_graph_hpp/__init__.py "")
INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/dynamic_graph_hpp/__init__.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph_hpp/)
SET(FILES
supervisor.py
tools.py
ros_interface.py
factory.py
__init__.py)
FOREACH(F ${FILES})
PYTHON_INSTALL_ON_SITE("sot_hpp" ${F})
ENDFOREACH()
......@@ -40,8 +40,8 @@ namespace dynamicgraph {
template<>const std::string TypeNameHelper<typeid>::typeName = #typeid
ADD_KNOWN_TYPE(double);
ADD_KNOWN_TYPE(dg::Vector);
ADD_KNOWN_TYPE(dg::Matrix);
ADD_KNOWN_TYPE(Vector);
ADD_KNOWN_TYPE(Matrix);
ADD_KNOWN_TYPE(MatrixRotation);
ADD_KNOWN_TYPE(MatrixTwist);
ADD_KNOWN_TYPE(MatrixHomogeneous);
......
......@@ -23,6 +23,7 @@
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <sot/hpp/config.hh>
......@@ -50,6 +51,12 @@ namespace dynamicgraph {
" Add a signal\n";
addCommand ("addSignal", makeCommandVoid1
(*this, &Event::addSignal, docstring));
docstring =
"\n"
" Get list of signals\n";
addCommand ("list", new command::Getter<Event, std::string>
(*this, &Event::getSignalsByName, docstring));
}
~Event () {}
......@@ -68,6 +75,18 @@ namespace dynamicgraph {
triggers.push_back(&PoolStorage::getInstance()->getSignal (iss));
}
// Returns the Python string representation of the list of signal names.
std::string getSignalsByName () const
{
std::ostringstream oss;
oss << "(";
for (Triggers_t::const_iterator _sig = triggers.begin();
_sig != triggers.end(); ++_sig)
oss << '\'' << (*_sig)->getName() << "\', ";
oss << ")";
return oss.str();
}
private:
typedef SignalBase<int>* Trigger_t;
typedef std::vector<Trigger_t> Triggers_t;
......@@ -81,7 +100,6 @@ namespace dynamicgraph {
for (Triggers_t::const_iterator _s = triggers.begin();
_s != triggers.end(); ++_s)
(*_s)->recompute (time);
std::cout << "trigger: " << val << std::endl;
}
return ret;
}
......@@ -96,4 +114,4 @@ namespace dynamicgraph {
} // namespace hpp
} // namespace sot
} // namespace dynamicgraph
#endif // SOT_HPP_PATH_SAMPLER_HH
#endif // SOT_HPP_EVENT_HH
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of sot_hpp.
// sot_hpp is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// sot_hpp is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// sot_hpp. If not, see <http://www.gnu.org/licenses/>.
#include <../src/holonomic-constraint.hh>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/factory.h>
namespace dynamicgraph {
namespace sot {
namespace hpp {
using std::sin;
using std::cos;
double sinc (const double& t)
{
if (t < 1e-3) {
const double t2 = t*t;
return 1 - t2 / 6 + t2*t2 / 120;
} else
return sin (t) / t;
}
HolonomicConstraint::HolonomicConstraint (const std::string& name) :
Entity (name),
dim_ (0),
g1SIN(NULL, "HolonomicConstraint("+name+")::input(double)::g1"),
g2SIN(NULL, "HolonomicConstraint("+name+")::input(double)::g2"),
g3SIN(NULL, "HolonomicConstraint("+name+")::input(double)::g3"),
positionSIN (NULL, "HolonomicConstraint("+name+")::input(matrixHomo)::position"),
positionRefSIN(NULL, "HolonomicConstraint("+name+")::input(matrixHomo)::positionRef"),
velocityRefSIN(NULL, "HolonomicConstraint("+name+")::input(vector)::velocityRef"),
errorSOUT ("HolonomicConstraint("+name+")::output(vector)::error"),
controlSOUT ("HolonomicConstraint("+name+")::output(vector)::control"),
projectorSOUT("HolonomicConstraint("+name+")::output(matrix)::projector")
{
errorSOUT.setFunction
(boost::bind (&HolonomicConstraint::computeError, this, _1, _2));
controlSOUT.setFunction
(boost::bind (&HolonomicConstraint::computeControl, this, _1, _2));
projectorSOUT.setFunction
(boost::bind (&HolonomicConstraint::computeProjector, this, _1, _2));
signalRegistration (g1SIN);
signalRegistration (g2SIN);
signalRegistration (g3SIN);
signalRegistration (positionSIN );
signalRegistration (positionRefSIN);
signalRegistration (velocityRefSIN);
signalRegistration (errorSOUT );
signalRegistration (controlSOUT );
signalRegistration (projectorSOUT);
std::string docstring =
"\n"
" Set the number of DoF of the robot\n";
addCommand ("setNumberDoF", command::makeCommandVoid1
(*this, &HolonomicConstraint::setNumberDoF, docstring));
}
Vector& HolonomicConstraint::computeError (Vector& error, const int& time)
{
const MatrixHomogeneous& oMp = positionSIN .access (time);
const MatrixHomogeneous& oMpr = positionRefSIN.access (time);
MatrixHomogeneous pMpr = oMp.inverse() * oMpr;
Eigen::AngleAxisd aa (pMpr.linear());
error.resize(6);
error.head<3>() = pMpr.translation();
error.tail<3>() = aa.angle() * aa.axis();
return error;
}
Vector& HolonomicConstraint::computeControl (Vector& control , const int& time)
{
const Vector& vr = velocityRefSIN.access (time);
const MatrixHomogeneous& oMp = positionSIN .access (time);
const Vector& error = errorSOUT. access (time);
const double& g1 = g1SIN.access(time);
const double& g2 = g2SIN.access(time);
const double& g3 = g3SIN.access(time);
control.resize (6);
control.head<3>() = oMp.linear().row(0) * (vr[0] * cos (error[5]) + g1 * error[0]);
control.tail<3>() = oMp.linear().row(2) * (vr[5] + g3 * error[5] + g2 * vr[0] * sinc(error[5]) * error[1]);
assert (!control.hasNaN());
return control;
}
Matrix& HolonomicConstraint::computeProjector (Matrix& projector, const int& )
{
projector.resize(6, dim_);
projector.leftCols<6>().setIdentity();
projector.rightCols(dim_-6).setZero();
return projector;
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (HolonomicConstraint, "HolonomicConstraint");
} // namespace hpp
} // namespace sot
} // namespace dynamicgraph
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of sot_hpp.
// sot_hpp is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// sot_hpp is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// sot_hpp. If not, see <http://www.gnu.org/licenses/>.
#ifndef SOT_HPP_HOLONOMIC_CONSTRAINT_HH
# define SOT_HPP_HOLONOMIC_CONSTRAINT_HH
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/hpp/config.hh>
namespace dynamicgraph {
namespace sot {
namespace hpp {
/// Holonomic constraints
class SOT_HPP_DLLAPI HolonomicConstraint : public dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
HolonomicConstraint (const std::string& name);
~HolonomicConstraint () {}
/// Header documentation of the python class
virtual std::string getDocString () const
{
return
"Compute the control (v, w) of size 2 for a mobile base.\n"
" Signal velocityRef is the desired velocity (v_r, w_r) expressed in the desired frame.\n";
}
void setNumberDoF (const unsigned int& d)
{ dim_ = d; }
private:
Vector& computeError (Vector& error , const int& time);
Vector& computeControl (Vector& control , const int& time);
Matrix& computeProjector (Matrix& projector, const int& time);
unsigned int dim_;
SignalPtr <double, int> g1SIN;
SignalPtr <double, int> g2SIN;
SignalPtr <double, int> g3SIN;
SignalPtr <MatrixHomogeneous, int> positionSIN;
SignalPtr <MatrixHomogeneous, int> positionRefSIN;
SignalPtr <Vector, int> velocityRefSIN;
Signal <Vector, int> errorSOUT;
Signal <Vector, int> controlSOUT;
Signal <Matrix, int> projectorSOUT;
};
} // namespace hpp
} // namespace sot
} // namespace dynamicgraph
#endif // SOT_HPP_HOLONOMIC_CONSTRAINT_HH
......@@ -93,19 +93,19 @@ namespace dynamicgraph
const std::string& topic = values[2].value ();
if (type == "double")
entity.add<double> (signal, topic);
entity.add<double> (type, signal, topic);
else if (type == "unsigned")
entity.add<unsigned int> (signal, topic);
entity.add<unsigned int> (type, signal, topic);
else if (type == "matrix")
entity.add<dg::Matrix> (signal, topic);
entity.add<dg::Matrix> (type, signal, topic);
else if (type == "vector")
entity.add<dg::Vector> (signal, topic);
entity.add<dg::Vector> (type, signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3> (signal, topic);
entity.add<specific::Vector3> (type, signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (signal, topic);
entity.add<sot::MatrixHomogeneous> (type, signal, topic);
else if (type == "twist")
entity.add<specific::Twist> (signal, topic);
entity.add<specific::Twist> (type, signal, topic);
else
throw std::runtime_error("bad type");
return Value ();
......
......@@ -5,6 +5,7 @@
# include <boost/shared_ptr.hpp>
# include <boost/thread/mutex.hpp>
# include <boost/thread/condition_variable.hpp>
# include <dynamic-graph/entity.h>
# include <dynamic-graph/signal-time-dependent.h>
......@@ -70,13 +71,20 @@ namespace dynamicgraph
RosQueuedSubscribe* entity;
};
template <typename T>
template <typename T, int BufferSize>
struct BindedSignal : BindedSignalBase {
typedef dynamicgraph::Signal<T, int> Signal_t;
typedef boost::shared_ptr<Signal_t> SignalPtr_t;
typedef std::queue<T> Queue_t;
BindedSignal(RosQueuedSubscribe* e) : BindedSignalBase (e), init(false) {}
typedef std::vector<T> buffer_t;
typedef typename buffer_t::size_type size_type;
BindedSignal(RosQueuedSubscribe* e)
: BindedSignalBase (e)
, frontIdx(0)
, backIdx(0)
, buffer (BufferSize)
, init(false)
{}
~BindedSignal()
{
std::cout << signal->getName() << ": Delete" << std::endl;
......@@ -84,22 +92,51 @@ namespace dynamicgraph
clear();
}
/// See comments in reader and writer for details about synchronisation.
void clear ()
{
qmutex.lock();
if (!queue.empty()) last = queue.back();
queue = Queue_t();
qmutex.unlock();
// synchronize with method writer
wmutex.lock();
if (!empty()) {
if (backIdx == 0)
last = buffer[BufferSize-1];
else
last = buffer[backIdx-1];
}
// synchronize with method reader
rmutex.lock();
frontIdx = backIdx = 0;
fullCondition.notify_all ();
rmutex.unlock();
wmutex.unlock();
}
bool empty () const
{
return frontIdx == backIdx;
}
bool full () const
{
return ((backIdx + 1) % BufferSize) == frontIdx;
}
std::size_t size () const
size_type size () const
{
return queue.size();
if (frontIdx <= backIdx)
return backIdx - frontIdx;
else
return backIdx + BufferSize - frontIdx;
}
SignalPtr_t signal;
Queue_t queue;
boost::mutex qmutex;
/// Index of the next value to be read.
size_type frontIdx;
/// Index of the slot where to write next value (does not contain valid data).
size_type backIdx;
buffer_t buffer;
boost::mutex wmutex, rmutex;
boost::condition_variable fullCondition;
T last;
bool init;
......@@ -123,7 +160,6 @@ namespace dynamicgraph
virtual std::string getDocString () const;
void display (std::ostream& os) const;
void add (const std::string& signal, const std::string& topic);
void rm (const std::string& signal);
std::string list ();
void clear ();
......@@ -132,7 +168,7 @@ namespace dynamicgraph
std::size_t queueSize (const std::string& signal) const;
template <typename T>
void add (const std::string& signal, const std::string& topic);
void add (const std::string& type, const std::string& signal, const std::string& topic);
std::map<std::string, bindedSignal_t>&
bindedSignal ()
......
......@@ -11,21 +11,25 @@
# include "dynamic_graph_bridge_msgs/Vector.h"
namespace dg = dynamicgraph;
typedef boost::mutex::scoped_lock scoped_lock;
namespace dynamicgraph
{
namespace internal
{
static const int BUFFER_SIZE = 1 << 10;
template <typename T>
struct Add
{
void operator () (RosQueuedSubscribe& rosSubscribe,
const std::string& type,
const std::string& signal,
const std::string& topic)
{
typedef typename SotToRos<T>::sot_t sot_t;
typedef typename SotToRos<T>::ros_const_ptr_t ros_const_ptr_t;
typedef BindedSignal<sot_t> BindedSignal_t;
typedef BindedSignal<sot_t, BUFFER_SIZE> BindedSignal_t;
typedef typename BindedSignal_t::Signal_t Signal_t;
// Initialize the bindedSignal object.
......@@ -33,8 +37,8 @@ namespace dynamicgraph
SotToRos<T>::setDefault (bs->last);
// Initialize the signal.
boost::format signalName ("RosQueuedSubscribe(%1%)::%2%");
signalName % rosSubscribe.getName () % signal;
boost::format signalName ("RosQueuedSubscribe(%1%)::output(%2%)::%3%");
signalName % rosSubscribe.getName () % type % signal;
bs->signal.reset (new Signal_t (signalName.str ()));
bs->signal->setFunction (boost::bind(&BindedSignal_t::reader, bs, _1, _2));
......@@ -49,7 +53,7 @@ namespace dynamicgraph
// -> No message should be lost because of a full buffer
bs->subscriber =
boost::make_shared<ros::Subscriber>
(rosSubscribe.nh ().subscribe (topic, 50, callback));
(rosSubscribe.nh ().subscribe (topic, BUFFER_SIZE, callback));
RosQueuedSubscribe::bindedSignal_t bindedSignal (bs);
rosSubscribe.bindedSignal ()[signal] = bindedSignal;
......@@ -57,45 +61,55 @@ namespace dynamicgraph
};
// template <typename T, typename R>
template <typename T>
template <typename T, int N>
template <typename R>
void BindedSignal<T>::writer (const R& data)
void BindedSignal<T, N>::writer (const R& data)
{
T value;
converter (value, data);
// synchronize with method clear
boost::mutex::scoped_lock lock(wmutex);
boost::mutex dummy;
boost::unique_lock<boost::mutex> lock_dummy (dummy);
while (full()) {
fullCondition.wait (lock_dummy);
}
converter (buffer[backIdx], data);
// No need to synchronize with reader here because:
// - if the buffer was not empty, then it stays not empty,
// - if it was empty, then the current value will be used at next time. It
// means the transmission bandwidth is too low.
if (!init) {
last = value;
last = buffer[backIdx];
init = true;
}
qmutex.lock();
queue.push (value);
qmutex.unlock();
backIdx = (backIdx+1) % N;
}
template <typename T>
T& BindedSignal<T>::reader (T& data, int time)
template <typename T, int N>
T& BindedSignal<T, N>::reader (T& data, int time)
{
if (entity->readQueue_ == -1 || time < entity->readQueue_) {
// synchronize with method clear:
// If reading from the list cannot be done, then return last value.
scoped_lock lock(rmutex, boost::try_to_lock);
if (!lock.owns_lock() || entity->readQueue_ == -1 || time < entity->readQueue_) {
data = last;
} else {
qmutex.lock();
if (queue.empty())
if (empty())
data = last;
else {
data = queue.front();
queue.pop();
data = buffer[frontIdx];
frontIdx = (frontIdx + 1) % N;
last = data;
fullCondition.notify_all();
}
qmutex.unlock();
}
return data;
}
} // end of namespace internal.
template <typename T>
void RosQueuedSubscribe::add (const std::string& signal, const std::string& topic)
void RosQueuedSubscribe::add (const std::string& type, const std::string& signal, const std::string& topic)
{
internal::Add<T> () (*this, signal, topic);
internal::Add<T> () (*this, type, signal, topic);
}
} // end of namespace dynamicgraph.
......
from hpp.corbaserver.manipulation.constraint_graph_factory import ConstraintFactoryAbstract, GraphFactoryAbstract
from tools import Manifold, Grasp, idx, idx_zip, OpFrame, EEPosture
from tools import Manifold, Grasp, OpFrame, EEPosture
from dynamic_graph.sot.core import SOT
class Affordance(object):
def __init__ (self, gripper, handle, **kwargs):
self.gripper = gripper
self.handle = handle
self.setControl (**kwargs)
def setControl (self, refOpen, refClose, openType = "position", closeType="position"):
if openType != "position" or closeType != "position":
raise NotImplementedError ("Only position control is implemented for gripper opening/closure.")
self.controlType = {
"open": openType,
"close": closeType,
}
self.ref = {
"open": refOpen,
"close": refClose,
}
class TaskFactory(ConstraintFactoryAbstract):
gfields = ('grasp', 'pregrasp', 'gripper_open', 'gripper_close')
pfields = ()
......@@ -9,12 +26,26 @@ class TaskFactory(ConstraintFactoryAbstract):