Test new borinot arm

crocoddyl/__init__.py

@@ -29,7 +29,7 @@ from .impact import (ActionDataImpact, ActionModelImpact, CostModelImpactCoM, Co
 from .integrated_action import (IntegratedActionDataEuler, IntegratedActionDataRK4, IntegratedActionModelEuler,
                                 IntegratedActionModelRK4)
 from .kkt import SolverKKT
-from .robots import getTalosPathFromRos, loadHyQ, loadTalos, loadTalosArm, loadTalosLegs, loadKinton, loadKintonArm
+from .robots import getTalosPathFromRos, loadHyQ, loadTalos, loadTalosArm, loadTalosLegs, loadKinton, loadKintonArm, loadBorinotArm
 from .shooting import ShootingProblem
 from .solver import SolverAbstract
 from .state import StateAbstract, StateNumDiff, StatePinocchio, StateVector

crocoddyl/plots.py

@@ -24,10 +24,9 @@ class PlotUAM:
         axs[1, 0].set_title('Motor 3')
         axs[1, 1].plot(t,self.PlotDataType.control[:,3])
         axs[1, 1].set_title('Motor 4')
-
         return fig,axs
 
-    def plotActuation(self):
+    def plotFlyingPlatformActuation(self):
         fig, axs = plt.subplots(1,2, figsize=(15,10))
         fig.suptitle('Motor forces')
         t = self.PlotDataType.t
@@ -35,11 +34,18 @@ class PlotUAM:
         axs[0].set_title('Moments')
         axs[0].legend(['Mx','My','Mz'])
         axs[1].plot(t,self.PlotDataType.thrust)
-        axs[1].set_title('Motor 2')
-
-
+        axs[1].set_title('Thrust')
         return fig,axs
 
+    # def plotArmActuation(self):
+    #     fig, axs = plt.subplot(1,1figsize=(15,10))
+    #     fig.suptitle('Motor forces')
+    #     t = self.PlotDataType.t
+    #     axs[0].plot(t,self.PlotDataType.M[:,0], t,self.PlotDataType.M[:,1], t,self.PlotDataType.M[:,2])
+    #     axs[0].set_title('Moments')
+    #     axs[0].legend(['M1','M2','M3','M4','M5','M6'])
+    #     return fig,axs
+
 class PlotDataUAM():
     def __init__(self, model):
         self.t = np.arange(0, model.knots*model.dt, model.dt)
@@ -50,7 +56,7 @@ class PlotDataUAM():
         My = model.d*(-self.control[:,0]+self.control[:,1]-self.control[:,2]+self.control[:,3])
         Mz = model.cm/model.cf*(-self.control[:,0]-self.control[:,1]+self.control[:,2]+self.control[:,3])
 
-        self.M = np.zeros([np.size(self.control,1), 3])
+        self.M = np.zeros([np.size(self.control,0), 3])
         self.M[:, 0] = Mx
         self.M[:, 1] = My
         self.M[:, 2] = Mz

crocoddyl/robots.py

@@ -133,7 +133,7 @@ def loadKinton(modelPath='/opt/openrobots/share/example-robot-data'):
     robot.q0.flat[7:] = [0, 0, 0, 0, 0, 0]
     robot.model.referenceConfigurations["initial_pose"] = robot.q0
     robot.q0.flat[7:] = [0, 0.2, 1.7, 2, 0, 0]
-    robot.model.referenceConfigurations["centered"] = robot.q0 
+    robot.model.referenceConfigurations["centered"] = robot.q0
     return robot
 
 def loadKintonArm(modelPath='/opt/openrobots/share/example-robot-data'):
@@ -141,3 +141,10 @@ def loadKintonArm(modelPath='/opt/openrobots/share/example-robot-data'):
     URDF_SUBPATH = "/kinton_description/urdf/" + URDF_FILENAME
     robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
     return robot
+
+def loadBorinotArm(modelPath='/opt/openrobots/share/example-robot-data'):
+    URDF_FILENAME = "borinot_arm.urdf"
+    URDF_SUBPATH = "/borinot_arm/urdf/" + URDF_FILENAME
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
+    robot.q0.flat = [np.pi,0]
+    return robot

examples/notebooks/kinton_fixed_base.ipynb

@@ -169,7 +169,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython2",
-   "version": "2.7.12"
+   "version": "2.7.16"
   }
  },
  "nbformat": 4,