Merge pull request #1208 from cmastalli/topic/quadruped-biped-examples

Author: cmastalli

bindings/python/crocoddyl/utils/biped.py

@@ -386,10 +386,22 @@ def createSwingFootModel(
         if self._integrator == "euler":
             model = crocoddyl.IntegratedActionModelEuler(dmodel, control, timeStep)
         elif self._integrator == "rk4":
-            model = crocoddyl.IntegratedActionModelRK4(dmodel, control, timeStep)
+            model = crocoddyl.IntegratedActionModelRK(
+                dmodel, control, crocoddyl.RKType.four, timeStep
+            )
+        elif self._integrator == "rk3":
+            model = crocoddyl.IntegratedActionModelRK(
+                dmodel, control, crocoddyl.RKType.three, timeStep
+            )
+        elif self._integrator == "rk2":
+            model = crocoddyl.IntegratedActionModelRK(
+                dmodel, control, crocoddyl.RKType.two, timeStep
+            )
+        else:
+            model = crocoddyl.IntegratedActionModelEuler(dmodel, control, timeStep)
         return model
 
-    def createFootSwitchModel(self, supportFootIds, swingFootTask, pseudoImpulse=True):
+    def createFootSwitchModel(self, supportFootIds, swingFootTask, pseudoImpulse=False):
         """Action model for a foot switch phase.
 
         :param supportFootIds: Ids of the constrained feet
@@ -512,15 +524,15 @@ def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
             )
         if self._integrator == "euler":
             model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.0)
-        elif self.integrator == "rk4":
+        elif self._integrator == "rk4":
             model = crocoddyl.IntegratedActionModelRK(
                 dmodel, crocoddyl.RKType.four, 0.0
             )
-        elif self.integrator == "rk3":
+        elif self._integrator == "rk3":
             model = crocoddyl.IntegratedActionModelRK(
                 dmodel, crocoddyl.RKType.three, 0.0
             )
-        elif self.integrator == "rk2":
+        elif self._integrator == "rk2":
             model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.two, 0.0)
         else:
             model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.0)

bindings/python/crocoddyl/utils/quadruped.py

@@ -127,7 +127,7 @@ def createCoMGoalProblem(self, x0, comGoTo, timeStep, numKnots):
         :return shooting problem
         """
         # Compute the current foot positions
-        q0 = self.rmodel.referenceConfigurations["standing"]
+        q0 = x0[: self.state.nq]
         pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
         pinocchio.updateFramePlacements(self.rmodel, self.rdata)
         com0 = pinocchio.centerOfMass(self.rmodel, self.rdata, q0)
@@ -739,7 +739,19 @@ def createSwingFootModel(
         if self._integrator == "euler":
             model = crocoddyl.IntegratedActionModelEuler(dmodel, control, timeStep)
         elif self._integrator == "rk4":
-            model = crocoddyl.IntegratedActionModelRK4(dmodel, control, timeStep)
+            model = crocoddyl.IntegratedActionModelRK(
+                dmodel, control, crocoddyl.RKType.four, timeStep
+            )
+        elif self._integrator == "rk3":
+            model = crocoddyl.IntegratedActionModelRK(
+                dmodel, control, crocoddyl.RKType.three, timeStep
+            )
+        elif self._integrator == "rk2":
+            model = crocoddyl.IntegratedActionModelRK(
+                dmodel, control, crocoddyl.RKType.two, timeStep
+            )
+        else:
+            model = crocoddyl.IntegratedActionModelEuler(dmodel, control, timeStep)
         return model
 
     def createFootSwitchModel(self, supportFootIds, swingFootTask, pseudoImpulse=False):
@@ -864,15 +876,15 @@ def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
             )
         if self._integrator == "euler":
             model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.0)
-        elif self.integrator == "rk4":
+        elif self._integrator == "rk4":
             model = crocoddyl.IntegratedActionModelRK(
                 dmodel, crocoddyl.RKType.four, 0.0
             )
-        elif self.integrator == "rk3":
+        elif self._integrator == "rk3":
             model = crocoddyl.IntegratedActionModelRK(
                 dmodel, crocoddyl.RKType.three, 0.0
             )
-        elif self.integrator == "rk2":
+        elif self._integrator == "rk2":
             model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.two, 0.0)
         else:
             model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.0)