This commit is contained in:
Pham Tuan Kiet 2022-02-13 16:00:08 +01:00
parent 35b96e504e
commit 6fad9ec825
10 changed files with 227 additions and 35 deletions

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

76
Code/ball.py Normal file
View file

@ -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()

14
Code/example_ball.py Normal file
View file

@ -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()

View file

@ -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
dt = 2e-3
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)

View file

@ -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,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:

113
Code/robot.py Normal file
View file

@ -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()