diff --git a/Code/__pycache__/ball.cpython-39.pyc b/Code/__pycache__/ball.cpython-39.pyc new file mode 100644 index 0000000..1068200 Binary files /dev/null and b/Code/__pycache__/ball.cpython-39.pyc differ diff --git a/Code/__pycache__/display.cpython-39.pyc b/Code/__pycache__/display.cpython-39.pyc new file mode 100644 index 0000000..34ee5e9 Binary files /dev/null and b/Code/__pycache__/display.cpython-39.pyc differ diff --git a/Code/__pycache__/leggedrobot.cpython-39.pyc b/Code/__pycache__/leggedrobot.cpython-39.pyc new file mode 100644 index 0000000..ccafe0e Binary files /dev/null and b/Code/__pycache__/leggedrobot.cpython-39.pyc differ diff --git a/Code/__pycache__/robot_arm.cpython-39.pyc b/Code/__pycache__/robot_arm.cpython-39.pyc new file mode 100644 index 0000000..eff4f29 Binary files /dev/null and b/Code/__pycache__/robot_arm.cpython-39.pyc differ diff --git a/Code/__pycache__/robotleg.cpython-39.pyc b/Code/__pycache__/robotleg.cpython-39.pyc new file mode 100644 index 0000000..f23f2da Binary files /dev/null and b/Code/__pycache__/robotleg.cpython-39.pyc differ diff --git a/Code/ball.py b/Code/ball.py new file mode 100644 index 0000000..2a4b972 --- /dev/null +++ b/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() + + diff --git a/Code/example_ball.py b/Code/example_ball.py new file mode 100644 index 0000000..8779f49 --- /dev/null +++ b/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() + + diff --git a/Code/example_robot.py b/Code/example_robot.py index 74781a3..4ff39df 100644 --- a/Code/example_robot.py +++ b/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 -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) +dt = 2e-3 +q = neutral (robot.model) +robot.display(q) diff --git a/Code/leggedrobot.py b/Code/leggedrobot.py index 5fed7cd..bc31a82 100644 --- a/Code/leggedrobot.py +++ b/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 -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 +from pinocchio import forwardKinematics, Inertia, JointModelRX,JointModelPY,JointModelPZ, Model, SE3,neutral 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,21 +67,22 @@ 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: visual.place( self.viewer,self.data.oMi[visual.jointParent] ) - self.viewer.viewer.gui.refresh() \ No newline at end of file + self.viewer.viewer.gui.refresh() diff --git a/Code/robot.py b/Code/robot.py new file mode 100644 index 0000000..7d325c0 --- /dev/null +++ b/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()