ball add

Code/ball.py

@@ -0,0 +1,76 @@
+from pinocchio.utils import *
+from pinocchio.explog import exp,log
+from numpy.linalg import pinv,norm
+from pinocchio import forwardKinematics, Inertia, JointModelRX, Model, neutral, SE3,JointModelPX,JointModelPZ
+import gepetto.corbaserver
+from display import Display
+import numpy as np
+import eigenpy
+import time
+eigenpy.switchToNumpyArray()
+
+class Visual:
+    def __init__(self,name,jointParent,placement):
+        self.name = name                  # Name in gepetto viewer
+        self.jointParent = jointParent    # ID (int) of the joint 
+        self.placement = placement        # placement of the body wrt joint, i.e. bodyMjoint
+    def place(self,display,oMjoint):
+        oMbody = oMjoint*self.placement
+        display.place(self.name,oMbody,False)
+
+class Ball:
+    def __init__(self,angle=np.pi/3,speed=5):
+        self.angle=angle
+        self.speed=speed
+        
+        self.viewer = Display()
+        self.visuals = []
+        self.model = Model ()
+        self.Create()
+        self.data = self.model.createData()
+        self.Throw()
+        self.q0 = zero(self.model.nq) 
+
+    def Create(self,rootId=0, jointPlacement=SE3.Identity()):
+        color   = [red,green,blue,transparency] = [1,1,0.78,1.0]
+        colorred = [1.0,0.0,0.0,1.0]
+
+        jointId = rootId
+
+        jointName          = "BallX"
+        jointPlacement     = SE3(eye(3),np.array( [0, 0, 2] ))
+        joint              = JointModelPX()
+        jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
+        #self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
+        
+        jointName          = "BallZ"
+        jointPlacement     = SE3(eye(3),np.array( [0, 0, 0] ))
+        joint              = JointModelPZ()
+        jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
+        self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
+        
+        self.viewer.viewer.gui.addSphere('world/' +'ball', 0.3,colorred)
+        self.visuals.append( Visual('world/' +'ball',jointId,SE3.Identity()) )
+        
+    def Throw(self):
+        dt = 1e-3
+        q = neutral (self.model)
+        v = np.array (self.model.nv * [0.0])
+        v1 = np.array (self.model.nv * [0.0])
+        v1[1]=-0.0098
+        v[0]=self.speed*np.cos(self.angle)
+        v[1]=self.speed*np.sin(self.angle)
+        print(v)
+        while (q[1]>-2):
+            q+=v*dt
+            v+=v1
+            self.display(q)
+            time.sleep(10*dt)
+        
+    def display(self,q):
+        forwardKinematics(self.model,self.data,q)
+        for visual in self.visuals:
+            visual.place( self.viewer,self.data.oMi[visual.jointParent] )
+        self.viewer.viewer.gui.refresh()
+
+

Code/example_ball.py

@@ -0,0 +1,14 @@
+from numpy.linalg import pinv,norm
+from pinocchio import neutral
+from pinocchio.explog import exp,log
+from display import Display
+import eigenpy
+from ball import Ball
+import eigenpy
+import numpy as np
+import time
+
+# Create a 7DOF robot.
+robot = Ball()
+
+

Code/example_robot.py

@@ -1,27 +1,20 @@
 # Create a 7DOF manipulator robot and moves it along a constant (random) velocity during 10secs.
-import numpy as np
 from numpy.linalg import pinv,norm
 from pinocchio import neutral
-from robotleg import Leg
+from pinocchio.explog import exp,log
+from display import Display
+import eigenpy
+from leggedrobot import Robot
+import eigenpy
 import time
 
 # Create a 7DOF robot.
-robot = Leg()
+robot = Robot()
 
 # Hide the floor.
 robot.viewer.viewer.gui.setVisibility('world/floor','OFF')
 
 # Move the robot during 10secs at velocity v.
-dt = 1e-3
+dt = 2e-3
-for j in range (robot.model.nv):
-    v = np.array (robot.model.nv * [0])
-    v [j] = 1
 q = neutral (robot.model)
-    for i in range(1000):
-        q += v*dt
 robot.display(q)
-        time.sleep(dt)
-    for i in range(1000):
-        q -= v*dt
-        robot.display(q)
-        time.sleep(dt)

Code/leggedrobot.py

@@ -1,19 +1,12 @@
 from pinocchio.utils import *
 from pinocchio.explog import exp,log
 from numpy.linalg import pinv,norm
-from pinocchio import forwardKinematics, Inertia, JointModelRX, Model, SE3
+from pinocchio import forwardKinematics, Inertia, JointModelRX,JointModelPY,JointModelPZ, Model, SE3,neutral
-import gepetto.corbaserver
-from display import Display
-import eigenpy
-eigenpy.switchToNumpyArray()
-
-from pinocchio.utils import *
-from pinocchio.explog import exp,log
-from numpy.linalg import pinv,norm
-from pinocchio import forwardKinematics, Inertia, JointModelRX, Model, SE3
 import gepetto.corbaserver
+import time
 from display import Display
 import eigenpy
+import numpy as np
 eigenpy.switchToNumpyArray()
 
 class Visual:
@@ -30,7 +23,7 @@ class Robot():
         self.tibia = 1.
         self.feetsize = (.2,.5,.1)
         self.feet = 0.2
-        self.leglen=1.+1.+.25
+        self.leglen=1.+1.+.3
         self.legdist=0.5
 
         self.viewer = Display()
@@ -39,8 +32,10 @@ class Robot():
         self.Create()
         self.data = self.model.createData()
         self.q0 = zero(self.model.nq)
+        #print(self.model)
+        #self.Walk(10)
 
-    def CreateLeg(self,rootId=0,name='',jointPlacement):
+    def CreateLeg(self,rootId=0,name='',jointPlacement=SE3.Identity()):
         color   = [red,green,blue,transparency] = [1,1,0.78,1.0]
         colorred = [1.0,0.0,0.0,1.0]
 
@@ -52,7 +47,7 @@ class Robot():
         joint              = JointModelRX()
         jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
         self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
-        self.viewer.viewer.gui.addSphere('world/'  +name+ 'hip', 0.3,colorred)
+        self.viewer.viewer.gui.addSphere('world/'  +name+ 'hip', 0.1,colorred)
         self.visuals.append( Visual('world/'  + name+'hip',jointId,SE3.Identity()) )
         self.viewer.viewer.gui.addBox('world/'  +name+ 'thigh', .1,.1,.5,color)
         self.visuals.append( Visual('world/' + name+'thigh',jointId,SE3(eye(3),np.array([0., 0., -.5]))))
@@ -62,7 +57,7 @@ class Robot():
         joint              = JointModelRX()
         jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
         self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
-        self.viewer.viewer.gui.addSphere('world/'  +name+ 'knee', 0.3,colorred)
+        self.viewer.viewer.gui.addSphere('world/'  +name+ 'knee', 0.1,colorred)
         self.visuals.append( Visual('world/'  + name+'knee',jointId,SE3.Identity()) )
         self.viewer.viewer.gui.addBox('world/'  +name+ 'tibia', .1,.1,.5,color)
         self.visuals.append( Visual('world/'  +name+ 'tibia',jointId,SE3(eye(3),np.array([0., 0., -.5]))))
@@ -72,19 +67,20 @@ class Robot():
         joint              = JointModelRX()
         jointId = self.model.addJoint(jointId,joint,jointPlacement,jointName)
         self.model.appendBodyToJoint(jointId,Inertia.Random(),SE3.Identity())
-        self.viewer.viewer.gui.addSphere('world/'  + name+'ankle', 0.3,colorred)
+        self.viewer.viewer.gui.addSphere('world/'  + name+'ankle', 0.1,colorred)
         self.visuals.append( Visual('world/' + name+'ankle',jointId,SE3.Identity()) )
         self.viewer.viewer.gui.addBox('world/' +name+ 'feet', .25,.5,.1,color)
         self.visuals.append( Visual('world/' +name+ 'feet',jointId,SE3(eye(3),np.array([0., .1, -.25]))))
 
     def Crleft(self):
-        self.CreateLeg(name='left_',jointPlacement=pin.SE3(np.eye(3), np.array([0, self.legdist/2, self.leglen])))
+        self.CreateLeg(name='left_',jointPlacement=SE3(np.eye(3), np.array([self.legdist/2,0, self.leglen])))     
     def Crright(self):
-        self.CreateLeg(name='right_',jointPlacement=pin.SE3(np.eye(3), np.array([0, -self.legdist/2, self.leglen])))
+        self.CreateLeg(name='right_',jointPlacement=SE3(np.eye(3), np.array([-self.legdist/2,0, self.leglen])))
     def Create(self):
         self.Crleft()
         self.Crright()
     
+                    
     def display(self,q):
         forwardKinematics(self.model,self.data,q)
         for visual in self.visuals:

Code/robot.py

@@ -0,0 +1,113 @@
+from pinocchio.utils import *
+from pinocchio.explog import exp,log
+from numpy.linalg import pinv,norm
+from pinocchio import forwardKinematics, Inertia, JointModelRX, Model, SE3
+import gepetto.corbaserver
+from display import Display
+import pinocchio as pin
+import eigenpy
+import hppfcl
+from pinocchio.visualize import GepettoVisualizer, MeshcatVisualizer
+
+eigenpy.switchToNumpyArray()
+
+color   = [red,green,blue,transparency] = [1,1,0.78,1.0]
+red = [1.0,0.0,0.0,1.0]
+
+class Joint:
+    def __init__(self,name,parent=None,rootid=0, **kargs):
+        self.name=name
+        self.joint_models = kwargs.get("joint_models", [pin.JointModelRX()])
+        self.placement = kwargs.get("placement", self.__calculate_placement(parent))
+        self.ids = []
+        self.geo_ids = []
+        self.inertias = kwargs.get("inertias")
+        
+        #check if parent =0
+        
+        if not self.inertias:
+            self.inertias = [None] * len(self.joint_models)
+            for i in range(len(self.joint_models)):
+                self.inertias[i] = pin.Inertia.Random()
+        for i, (joint_model, inertia) in enumerate(
+            zip(self.joint_models, self.inertias)
+        ):
+            current_parent = None
+            current_placement = None
+            if i == 0:
+                current_parent = parent.id if parent else root_id
+                current_placement = self.placement
+            else:
+                current_parent = self.ids[-1]
+                current_placement = pin.SE3.Identity()
+            self.ids.append(
+                model.addJoint(
+                    current_parent,
+                    joint_model,
+                    current_placement,
+                    self.name,
+                )
+            model.appendBodyToJoint(self.ids[-1], inertia, pin.SE3.Identity())
+        self.id = self.ids[-1]
+        self.sphere_radius = kwargs.get("sphere_radius", 0.1)
+        self.sphere = None
+        if self.sphere_radius:
+            self.sphere_name = kwargs.get("sphere_name", self.name)
+            self.sphere_color = kwargs.get("sphere_color", red)
+            self.sphere_placement = kwargs.get("sphere_placement", pin.SE3.Identity())
+        self.sphere = self.__create_geo_obj(
+                self.sphere_name,
+                self.sphere_placement,
+                self.sphere_color,
+                hppfcl.Sphere,
+                self.sphere_radius,
+            )
+            self.geo_ids.append(geo_model.addGeometryObject(self.sphere))
+        self.box_x = kwargs.get("box_x", 0.1)
+        self.box_y = kwargs.get("box_y", 0.1)
+        self.box_z = kwargs.get("box_z", 0)
+        self.box = None
+        if self.box_x and self.box_y and self.box_z:
+            self.box_name = kwargs.get("box_name", f"box_{name}")
+            self.box_color = kwargs.get("box_color", WHITE)
+            # Placement of the geometry with respect to the joint frame
+            self.box_placement = kwargs.get(
+                "box_placement", pin.SE3(np.eye(3), np.array([0, 0, self.box_z / 2]))
+            )
+            self.box = self.__create_geo_obj(
+                self.box_name,
+                self.box_placement,
+                self.box_color,
+                hppfcl.Box,
+                self.box_x,
+                self.box_y,
+                self.box_z,
+            )
+            self.geo_ids.append(geo_model.addGeometryObject(self.box))
+        def __create_geo_obj(self, name, placement, color, factory, *nargs, **kwargs):
+        return pin.GeometryObject(
+            name=name,
+            parent_joint=self.id,
+            collision_geometry=factory(*nargs, **kwargs),
+            placement=placement,
+            mesh_path="",
+            mesh_scale=np.ones(3),
+            override_material=False,
+            mesh_color=color,
+        )
+        def __calculate_placement(self, parent=None):
+        if parent is None:
+            return pin.SE3.Identity()
+        else:
+            parent_z = 0
+            if parent.box_z != 0:
+                parent_z = parent.box_z
+        return pin.SE3(np.eye(3), np.array([0, 0, parent_z]))
+
+class Robot:
+    def __init__(self, root_id=0, show_origin=True, add_plane=True):
+        self.viewer = Display()
+        self.visuals = []
+        self.model = Model()
+        collision_model = pin.GeometryModel()
+        visual_model = pin.GeometryModel()