making feature_constraints compile

feature_constraints/include/feature_constraints/Controller.h

@@ -10,17 +10,31 @@
 
 #include <feature_constraints/FeatureConstraints.h>
 
+
+#define MAX_CONSTRAINTS 64
+
+
 // describes the instantaneous constraint command
 class Ranges
 {
 public:
-  Ranges(int size);
+  Ranges(int size=0);
   KDL::JntArray pos_lo;  //!< min values for the constraints
   KDL::JntArray pos_hi;  //!< max values for the constraints
   KDL::JntArray weight;  //!< weights for the constraints (0|1)
 };
 
 
+//! pseudo-inverse working matrices
+class PinvData
+{
+public:
+  Eigen::MatrixXd U, V;
+  Eigen::VectorXd Sp, tmp;
+  void resize(int size);
+};
+
+
 void control(KDL::JntArray& ydot, KDL::JntArray& weights,
 	     KDL::JntArray& chi_desired, KDL::JntArray& chi,
 	     Ranges& command, KDL::JntArray gains);
@@ -52,18 +66,22 @@ class Controller
   KDL::Jacobian J;
   KDL::JntArray singularValues;
   KDL::JntArray tmp;
+  PinvData tmp_pinv;
 
   // does non-realtime preparation work
   // assumes that constraints is set
-  void prepare();
+  void prepare(int max_constraints=MAX_CONSTRAINTS);
 
   // calls deriveConstraints(), control() and analyzeH()
   void update(KDL::Frame frame);
 };
 
+
 //! Analyze interaction matrix.
-void analyzeH(KDL::Jacobian Ht, KDL::Jacobian& J, KDL::JntArray& singularValues, double eps=1e-15);
+void analyzeH(PinvData& tmpdata, KDL::Jacobian Ht, KDL::Jacobian& J,
+		      KDL::JntArray& singularValues, double eps=1e-15);
 
+//! Do range-based control.
 void control(KDL::JntArray& ydot, KDL::JntArray& weights,
 	     KDL::JntArray& chi_desired, KDL::JntArray& chi,
 	     Ranges& command, KDL::JntArray gains);

feature_constraints/include/feature_constraints/Conversions.h

@@ -6,6 +6,7 @@
 #include <geometry_msgs/Vector3.h>
 #include <constraint_msgs/Feature.h>
 #include <constraint_msgs/Constraint.h>
+#include <constraint_msgs/ConstraintConfig.h>
 #include <constraint_msgs/ConstraintCommand.h>
 #include <constraint_msgs/ConstraintState.h>
 
@@ -18,6 +19,7 @@
 void fromMsg(const geometry_msgs::Vector3& msg, KDL::Vector vec);
 void fromMsg(const constraint_msgs::Feature& msg, Feature& feat);
 void fromMsg(const constraint_msgs::Constraint& msg, Constraint& c);
+void fromMsg(const constraint_msgs::ConstraintConfig& msg, std::vector<Constraint>& cc);
 void fromMsg(std::vector<double>& msg, KDL::JntArray& jnts);
 void fromMsg(constraint_msgs::ConstraintCommand& msg, Ranges& ranges);
 

feature_constraints/include/feature_constraints/FeatureConstraints.h

@@ -9,6 +9,10 @@
 #include <map>
 
 // TODO: having strings inside features, screws its realtime safety for creation! is that bad?
+//       (_no_ copy-on-write implementation for the GNU std::string.  :-( )
+
+#define STRING_SIZE 256
+
 
 // A tool- or workspace feature
 class Feature
@@ -19,10 +23,10 @@ class Feature
   KDL::Vector pos;     //!< position of the feature
   KDL::Vector dir;     //!< direction of the feature
 
-  Feature() {}
+  Feature() {name.reserve(STRING_SIZE);}
   Feature(std::string name, KDL::Vector pos,
           KDL::Vector dir, KDL::Vector dir2=KDL::Vector())
-    : name(name), pos(pos), dir(dir) {}
+    : name(name), pos(pos), dir(dir) {name.reserve(STRING_SIZE);}
 };
 
 
@@ -41,11 +45,12 @@ class Constraint
   FeatureFunc func;
   Feature tool_features[2], object_features[2];
 
-  Constraint() {}
+  Constraint() {name.reserve(STRING_SIZE);}
   Constraint(std::string name, FeatureFunc func,
              Feature tool_feature, Feature object_feature)
    : name(name), func(func)
   {
+	name.reserve(STRING_SIZE);
     tool_features[0] = tool_feature;
     object_features[0] = object_feature;
   }
@@ -54,6 +59,7 @@ class Constraint
              Feature tool_feature, Feature object_feature)
    : name(name), func(feature_functions_[func_name])
   {
+	name.reserve(STRING_SIZE);
     tool_features[0] = tool_feature;
     object_features[0] = object_feature;
   }
@@ -100,7 +106,7 @@ double pointing_at(KDL::Frame frame, Feature* tool_features, Feature* object_fea
 //! compute the values of the constraints, given a frame
 void evaluateConstraints(KDL::JntArray &values, KDL::Frame frame, const std::vector<Constraint> &constraints);
 
-//! compute the numerial derivative of the constraints around frame
+//! compute the numerical derivative of the constraints around frame
 void deriveConstraints(KDL::Jacobian& Ht, KDL::JntArray& values, KDL::Frame& frame,
                        std::vector<Constraint> &constraints, double dd,  KDL::JntArray& tmp);
 

feature_constraints/manifest.xml

@@ -14,7 +14,8 @@
   <depend package="constraint_msgs" />
 
   <export> 
-    <cpp cflags="-I${prefix}/include" /> 
+    <cpp cflags="-I${prefix}/include"
+         lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lfeature_constraints"/>
   </export> 
 
 </package>

feature_constraints/src/Controller.cpp

@@ -5,16 +5,19 @@
 #include <kdl/utilities/svd_eigen_HH.hpp>
 #include <Eigen/Core>
 
+
 using namespace KDL;
 using namespace Eigen;
 
+
 Ranges::Ranges(int size) :
   pos_lo(size), pos_hi(size), weight(size)
 {
 
 }
 
-void Controller::prepare()
+
+void Controller::prepare(int max_constraints)
 {
   int n = constraints.size();
 
@@ -29,16 +32,19 @@ void Controller::prepare()
   singularValues.resize(n);
 
   tmp.resize(n);
+  tmp_pinv.resize(max_constraints);  // maximum number of constraints
 }
 
+
 void Controller::update(KDL::Frame frame)
 {
   deriveConstraints(Ht, chi, frame, constraints, 0.01, tmp);
   control(ydot, weights, chi_desired, chi, command, gains);
-  analyzeH(Ht, J, singularValues, 1e-7);
+  analyzeH(tmp_pinv, Ht, J, singularValues, 1e-7);
   this->frame = frame;
 }
 
+
 //! new range-controller.
 //! this controller accepts ranges of accepted positions and
 //! lowers the weight to zero when inside that range. 
@@ -98,22 +104,23 @@ void control(KDL::JntArray& ydot, KDL::JntArray& weights,
 }
 
 
-// WARNING: this is NOT at all realtime-safe.
-void analyzeH(KDL::Jacobian& Ht, KDL::Jacobian& J, KDL::JntArray& singularValues, double eps=1e-15)
+void PinvData::resize(int size)
 {
-  int m=Ht.data.cols(), n=Ht.data.rows();
+  U.resize(size, 6);
+  V.resize(6,6);
+  Sp.resize(6);
+  tmp.resize(6);
+}
+
 
-  // NOTE: this could be realtime safe if these matrices had max. sizes.
-  MatrixXd U(m,n), V(n,n);
+void analyzeH(PinvData& tmp, KDL::Jacobian& Ht, KDL::Jacobian& J, KDL::JntArray& singularValues, double eps=1e-15)
+{
   VectorXd& S = singularValues.data;
-  VectorXd Sp(n), tmp(n);
 
-  svd_eigen_HH(Ht.data.transpose(), U, S, V, tmp);
+  svd_eigen_HH(Ht.data.transpose(), tmp.U, S, tmp.V, tmp.tmp);
 
-  for(int i=0; i <n; ++i)
-      Sp(i) = (fabs(S(i)) > eps) ? 1.0 / S(i) : 0.0;
+  for(int i=0; i < 6; ++i)
+      tmp.Sp(i) = (fabs((double) S(i)) > eps) ? 1.0 / S(i) : 0.0;
 
-  J.data = V * Sp.asDiagonal() * U.transpose();
+  J.data = tmp.V * tmp.Sp.asDiagonal() * tmp.U.transpose();
 }
-
-

feature_constraints/src/Conversions.cpp

@@ -46,6 +46,15 @@ void fromMsg(const constraint_msgs::Constraint& msg, Constraint& c)
   fromMsg(msg.world_feature, c.object_features[0]);
 }
 
+
+void fromMsg(const constraint_msgs::ConstraintConfig& msg, std::vector<Constraint>& cc)
+{
+  int num = msg.constraints.size();
+  cc.resize(num);
+  for(int i=0; i < num; i++)
+    fromMsg(msg.constraints[i], cc[i]);
+}
+
 Constraint fromMsg(const constraint_msgs::Constraint& msg)
 {
   return Constraint(msg.name, msg.function,
@@ -112,8 +121,8 @@ void toMsg(Controller& c, constraint_msgs::ConstraintState& c_msg)
   toMsg(c.frame, c_msg.pose);
   toMsg(c.chi, c_msg.chi);
   toMsg(c.chi_desired, c_msg.chi_desired);
-  toMsg(c.J, c_msg.jacobian);
   toMsg(c.weights, c_msg.weights);
+  toMsg(c.J, c_msg.jacobian);
   toMsg(c.singularValues, c_msg.singular_values);
 }