added 3d com support

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@814 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/IK/constraints/BalanceConstraint.cpp

@@ -16,14 +16,14 @@
 using namespace VirtualRobot;
 
 BalanceConstraint::BalanceConstraint(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SceneObjectSetPtr &contactNodes,
-                                     float tolerance, float minimumStability, float maxSupportDistance, bool supportPolygonUpdates) :
+                                     float tolerance, float minimumStability, float maxSupportDistance, bool supportPolygonUpdates, bool consderCoMHeight) :
     Constraint(joints)
 {
     initialize(robot, joints, bodies, contactNodes, tolerance, minimumStability, maxSupportDistance, supportPolygonUpdates);
 }
 
 BalanceConstraint::BalanceConstraint(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SupportPolygonPtr &supportPolygon,
-                                     float tolerance, float minimumStability, float maxSupportDistance, bool supportPolygonUpdates) :
+                                     float tolerance, float minimumStability, float maxSupportDistance, bool supportPolygonUpdates, bool consderCoMHeight) :
     Constraint(joints)
 {
     initialize(robot, joints, bodies, supportPolygon->getContactModels(), tolerance, minimumStability, maxSupportDistance, supportPolygonUpdates);
@@ -37,10 +37,22 @@ void BalanceConstraint::initialize(const RobotPtr &robot, const RobotNodeSetPtr
     this->maxSupportDistance = maxSupportDistance;
     this->tolerance = tolerance;
     this->supportPolygonUpdates = supportPolygonUpdates;
+    this->considerCoMHeight = considerCoMHeight;
 
     supportPolygon.reset(new SupportPolygon(contactNodes));
 
-    comIK.reset(new CoMIK(joints, bodies));
+    VR_INFO << "COM height:" << height << endl;
+    int dim = 2;
+    if (considerCoMHeight)
+    {
+        dim = 3;
+        setCoMHeight(height);
+    }
+    else{
+        height = robot->getCoMGlobal()(2);
+        setCoMHeight(height);
+    }
+    comIK.reset(new CoMIK(joints, bodies,RobotNodePtr(),dim));
 
     updateSupportPolygon();
 
@@ -55,7 +67,21 @@ void BalanceConstraint::updateSupportPolygon()
     THROW_VR_EXCEPTION_IF(!convexHull, "Empty support polygon for balance constraint");
 
     Eigen::Vector2f supportPolygonCenter = MathTools::getConvexHullCenter(convexHull);
-    comIK->setGoal(supportPolygonCenter, tolerance);
+
+    if (considerCoMHeight)
+    {
+        Eigen::Vector3f goal;
+        goal.head(2) = supportPolygonCenter;
+        goal(2) = height;
+        comIK->setGoal(goal, tolerance);
+        VR_INFO << "CoM Height of Inversed Robot set to:" << goal(2) << endl;
+    } else
+        comIK->setGoal(supportPolygonCenter, tolerance);
+}
+
+void BalanceConstraint::setCoMHeight(float currentheight)
+{
+    height = currentheight;
 }
 
 Eigen::MatrixXf BalanceConstraint::getJacobianMatrix()
@@ -83,7 +109,10 @@ Eigen::VectorXf BalanceConstraint::getError(float stepSize)
     }
     else
     {
-        return Eigen::Vector2f(0,0);
+        if (considerCoMHeight)
+            return Eigen::Vector3f(0,0,0);
+        else
+            return Eigen::Vector2f(0,0);
     }
 }
 

VirtualRobot/IK/constraints/BalanceConstraint.h

@@ -39,9 +39,9 @@ namespace VirtualRobot
     {
         public:
             BalanceConstraint(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SceneObjectSetPtr &contactNodes,
-                              float tolerance=0.1f, float minimumStability=0.5f, float maxSupportDistance=10.0f, bool supportPolygonUpdates=true);
+                              float tolerance=0.1f, float minimumStability=0.5f, float maxSupportDistance=10.0f, bool supportPolygonUpdates=true, bool consderCoMHeight = false);
             BalanceConstraint(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SupportPolygonPtr &supportPolygon,
-                              float tolerance=0.1f, float minimumStability=0.5f, float maxSupportDistance=10.0f, bool supportPolygonUpdates=true);
+                              float tolerance=0.1f, float minimumStability=0.5f, float maxSupportDistance=10.0f, bool supportPolygonUpdates=true, bool consderCoMHeight = false);
 
             Eigen::MatrixXf getJacobianMatrix();
             Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp);
@@ -53,7 +53,7 @@ namespace VirtualRobot
             SupportPolygonPtr getSupportPolygon();
 
             std::string getConstraintType();
-
+void        setCoMHeight(float height);
         protected:
             void initialize(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SceneObjectSetPtr &contactNodes,
                             float tolerance, float minimumStability, float maxSupportDistance, bool supportPolygonUpdates);
@@ -62,10 +62,15 @@ namespace VirtualRobot
 
             void visualizeSupportPolygon(SoSeparator *sep);
 
+
+
         protected:
             CoMIKPtr comIK;
             SupportPolygonPtr supportPolygon;
 
+            float height;
+            bool considerCoMHeight;
+
             RobotNodeSetPtr joints;
             RobotNodeSetPtr bodies;