Commit 847c9012 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[tp2] ivigit.

parent 0e9022be
This diff is collapsed.
This diff is collapsed.
'''
Load 4 times the UR5 model, plus a plate object on top of them, to feature a simple parallel robot.
No optimization, this file is just an example of how to load the models.
'''
import math
import time
import numpy as np
import pinocchio as pin
from load_ur5_parallel import load as loadUR5s
from scipy.optimize import fmin_bfgs
from numpy.linalg import norm,inv,pinv,svd,eig
from tp2.meshcat_viewer_wrapper import MeshcatVisualizer
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
robot = loadUR5s()
# Open the viewer
viz = MeshcatVisualizer(url='classical')
# Add a new object featuring the parallel robot tool plate.
[w, h, d] = [0.5, 0.5, 0.005]
color = [red, green, blue, transparency] = [1, 1, 0.78, .3]
viz.addBox('world/robot0/toolplate', [w, h, d], color)
Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))
viz.applyConfiguration('world/robot0/toolplate', Mtool)
import pinocchio as pin
import example_robot_data as robex
from tp2.meshcat_viewer_wrapper import MeshcatVisualizer
ROBOT_NAME = 'solo12'
#ROBOT_NAME = 'ur5'
robot = robex.load(ROBOT_NAME)
viz = MeshcatVisualizer(robot,url='classical')
viz.display(robot.q0)
'''
Stand-alone inverse geom in 3D. Given a reference translation <target> ,
it computes the configuration of the UR5 so that the end-effector position (3D)
matches the target. This is done using BFGS solver. While iterating to compute
the optimal configuration, the script also display the successive candidate
solution, hence moving the robot from the initial guess configuaration to the
reference target.
'''
import math
import time
import numpy as np
import pinocchio as pin
import example_robot_data as robex
from scipy.optimize import fmin_bfgs
from numpy.linalg import norm,inv,pinv,svd,eig
from tp2.meshcat_viewer_wrapper import MeshcatVisualizer
# --- Load robot model
robot = robex.load('solo12')
NQ = robot.model.nq
NV = robot.model.nv
# Open the viewer
viz = MeshcatVisualizer(robot,url='classical')
viz.display(robot.q0)
robot.feetIndexes = [ robot.model.getFrameId(frameName) for frameName in ['HR_FOOT','HL_FOOT','FR_FOOT','FL_FOOT' ] ]
# --- Add box to represent target
colors = ['red','blue','green','magenta']
for color in colors:
viz.addSphere("world/%s"%color, .05, color)
viz.addSphere("world/%s_des"%color, .05, color)
#
# OPTIM 6D #########################################################
#
targets = [
np.array( [-0.7, -0.2, 1.2]),
np.array( [-0.3, 0.5, 0.8]),
np.array( [ 0.3, 0.1, -0.1]),
np.array( [ 0.9, 0.9, 0.5])
]
for i in range(4): targets[i][2]+=1
def cost(q):
'''Compute score from a configuration'''
cost = 0.
for i in range(4):
p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation
cost += norm(p_i-targets[i])**2
return cost
def callback(q):
viz.applyConfiguration('world/box', Mtarget)
for i in range(4):
p_i = robot.framePlacement(q, robot.feetIndexes[i])
viz.applyConfiguration('world/%s'%colors[i], p_i)
viz.applyConfiguration('world/%s_des'%colors[i], list(targets[i])+[1,0,0,0])
viz.display(q)
time.sleep(1e-1)
Mtarget = pin.SE3(pin.utils.rotate('x',3.14/4), np.array([0.5, 0.1, 0.2])) # x,y,z
qopt = fmin_bfgs(cost, robot.q0, callback=callback)
'''
Stand-alone inverse geom in 3D. Given a reference translation <target> ,
it computes the configuration of the UR5 so that the end-effector position (3D)
matches the target. This is done using BFGS solver. While iterating to compute
the optimal configuration, the script also display the successive candidate
solution, hence moving the robot from the initial guess configuaration to the
reference target.
'''
import math
import time
import numpy as np
import pinocchio as pin
import example_robot_data as robex
from scipy.optimize import fmin_bfgs
from numpy.linalg import norm,inv,pinv,svd,eig
from tp2.meshcat_viewer_wrapper import MeshcatVisualizer
# --- Load robot model
robot = robex.load('ur5')
NQ = robot.model.nq
NV = robot.model.nv
# Open the viewer
viz = MeshcatVisualizer(robot,url='classical')
# Define an init config
robot.q0 = np.array([0, -3.14/2, 0, 0, 0, 0])
viz.display(robot.q0)
time.sleep(1)
print("Let's go to pdes.")
# --- Add ball to represent target
viz.addSphere("world/ball", .05, 'green')
viz.addBox("world/blue", [.08]*3, [.2, .2, 1., .5])
#
# OPTIM 3D #########################################################
#
def cost(q):
'''Compute score from a configuration'''
p = robot.placement(q, 6).translation
return norm(p - target)**2
def callback(q):
viz.applyConfiguration('world/ball', target.tolist()+[0,1,0,0])
viz.applyConfiguration('world/blue', robot.placement(q, 6))
viz.display(q)
time.sleep(1e-1)
target = np.array([0.5, 0.1, 0.2]) # x,y,z
qopt = fmin_bfgs(cost, robot.q0, callback=callback)
'''
Stand-alone inverse geom in 3D. Given a reference translation <target> ,
it computes the configuration of the UR5 so that the end-effector position (3D)
matches the target. This is done using BFGS solver. While iterating to compute
the optimal configuration, the script also display the successive candidate
solution, hence moving the robot from the initial guess configuaration to the
reference target.
'''
import math
import time
import numpy as np
import pinocchio as pin
import example_robot_data as robex
from scipy.optimize import fmin_bfgs
from numpy.linalg import norm,inv,pinv,svd,eig
from tp2.meshcat_viewer_wrapper import MeshcatVisualizer
# --- Load robot model
robot = robex.load('ur5')
NQ = robot.model.nq
NV = robot.model.nv
# Open the viewer
viz = MeshcatVisualizer(robot,url='classical')
# Define an init config
robot.q0 = np.array([0, -3.14/2, 0, 0, 0, 0])
viz.display(robot.q0)
time.sleep(1)
print("Let's go to pdes.")
# --- Add box to represent target
viz.addBox("world/box", [.05,.1,.2], [1., .2, .2, .5])
viz.addBox("world/blue", [.05,.1,.2], [.2, .2, 1., .5])
#
# OPTIM 6D #########################################################
#
def cost(q):
'''Compute score from a configuration'''
M = robot.placement(q, 6)
return norm(pin.log(M.inverse() * Mtarget).vector)
def callback(q):
viz.applyConfiguration('world/box', Mtarget)
viz.applyConfiguration('world/blue', robot.placement(q, 6))
viz.display(q)
time.sleep(1e-1)
Mtarget = pin.SE3(pin.utils.rotate('x',3.14/4), np.array([0.5, 0.1, 0.2])) # x,y,z
qopt = fmin_bfgs(cost, robot.q0, callback=callback)
print('The robot finally reached effector placement at\n',robot.placement(qopt,6))
import numpy as np
import pinocchio as pin
import example_robot_data as robex
def load():
robot = robex.load('ur5')
nbRobots = 4
models = [ robot.model.copy() for _ in range(nbRobots) ]
vmodels = [ robot.visual_model.copy() for _ in range(nbRobots) ]
# Build the kinematic model by assembling 4 UR5
fullmodel = pin.Model()
for irobot,model in enumerate(models):
# Change frame names
for i,f in enumerate(model.frames):
f.name = '%s_#%d' % (f.name,irobot)
# Change joint names
for i,n in enumerate(model.names):
model.names[i] = '%s_#%d' % (n,irobot)
# Choose the placement of the new arm to be added
Mt = pin.SE3(np.eye(3), np.array([.3, 0, 0.])) # First robot is simply translated
basePlacement = pin.SE3(pin.utils.rotate('z', np.pi*irobot / 2), np.zeros(3)) * Mt
# Append the kinematic model
fullmodel = pin.appendModel(fullmodel,model,0,basePlacement)
# Build the geometry model
fullvmodel = pin.GeometryModel()
for irobot,(model,vmodel) in enumerate(zip(models,vmodels)):
# Change geometry names
for i,g in enumerate(vmodel.geometryObjects):
# Change the name to avoid conflict
g.name = '%s_#%d' % (g.name,irobot)
# Refere to new parent names in the full kinematic tree
g.parentFrame = fullmodel.getFrameId(model.frames[g.parentFrame].name)
g.parentJoint = fullmodel.getJointId(model.names[g.parentJoint])
# Append the geometry model
fullvmodel.addGeometryObject(g)
#print('add %s on frame %d "%s"' % (g.name, g.parentFrame, fullmodel.frames[g.parentFrame].name))
fullrobot = pin.RobotWrapper(fullmodel,fullvmodel,fullvmodel)
#fullrobot.q0 = np.array([-0.375, -1.2 , 1.71 , -0.51 , -0.375, 0. ]*4)
fullrobot.q0 = np.array([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0]*nbRobots)
return fullrobot
if __name__ == "__main__":
from meshcat_viewer_wrapper import MeshcatVisualizer
robot = load()
viz = MeshcatVisualizer(robot,url='classical')
viz.display(robot.q0)
from . import colors
from .visualizer import MeshcatVisualizer
import meshcat
def rgb2int(r,g,b):
'''Convert 3 integers (chars) 0<= r,g,b < 256 into one single integer = 256**2*r+256*g+b,
as expected by Meshcat. '''
return int(r*256**2 + g*256 + b)
def material( color, transparent=False):
mat = meshcat.geometry.MeshPhongMaterial()
mat.color = color
mat.transparent = transparent
return mat
red = material(color=rgb2int(255, 0, 0),transparent=False)
blue = material(color=rgb2int( 0, 0,255),transparent=False)
green = material(color=rgb2int( 0,255, 0),transparent=False)
yellow = material(color=rgb2int(255,255, 0),transparent=False)
magenta = material(color=rgb2int(255, 0,255),transparent=False)
cyan = material(color=rgb2int( 0,255,255),transparent=False)
white = material(color=rgb2int(250,250,250),transparent=False)
black = material(color=rgb2int( 5, 5, 5),transparent=False)
grey = material(color=rgb2int(120,120,120),transparent=False)
colormap = {
'red':red,
'blue': blue,
'green': green,
'yellow': yellow,
'magenta': magenta,
'cyan': cyan,
'black': black,
'white': white,
'grey': grey
}
import numpy as np
import meshcat
import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer as PMV
from . import colors
def materialFromColor(color):
if isinstance(color,meshcat.geometry.MeshPhongMaterial): return color
elif isinstance(color,str): material=colors.colormap[color]
elif isinstance(color,list):
from .colors import rgb2int
material = meshcat.geometry.MeshPhongMaterial()
material.color = rgb2int(*[ int(c*255) for c in color[:3] ] )
if len(color)==3:
material.transparent = False
else:
material.transparent = color[3]<1
material.opacity = float(color[3])
elif color is None:
import random
material = random.sample(list(colors.colormap),1)[0]
else:
material = colors.black
return material
class MeshcatVisualizer(PMV):
def __init__(self,robot=None,model=None,collision_model=None,visual_model=None,url=None):
if robot is not None:
PMV.__init__(self,robot.model,robot.collision_model,robot.visual_model)
elif model is not None:
PMV.__init__(self,model,collision_model,visual_model)
if url is not None:
if url=='classical': url = 'tcp://127.0.0.1:6000'
print('Wrapper tries to connect to server <%s>'%url)
server = meshcat.Visualizer(zmq_url=url)
else:
server = None
if robot is not None or model is not None:
self.initViewer(loadModel=True,viewer=server)
else:
self.viewer = server if server is not None else meshcat.Visualizer()
def addSphere(self,name,radius,color):
material=materialFromColor(color)
self.viewer[name].set_object(meshcat.geometry.Sphere(radius),material)
def addCylinder(self,name,length,radius,color=None):
material=materialFromColor(color)
self.viewer[name].set_object(meshcat.geometry.Cylinder(length,radius),material)
def addBox(self,name,dims,color):
material=materialFromColor(color)
self.viewer[name].set_object(meshcat.geometry.Box(dims),material)
def applyConfiguration(self,name,placement):
if isinstance(placement,list) or isinstance(placement,tuple): placement = np.array(placement)
if isinstance(placement,pin.SE3):
R,p = placement.rotation,placement.translation
T = np.r_[np.c_[R,p],[[0,0,0,1]]]
elif isinstance(placement,np.ndarray):
if placement.shape == (7,): # XYZ-quat
R = pin.Quaternion(np.reshape(placement[3:],[4,1])).matrix()
p = placement[:3]
T = np.r_[np.c_[R,p],[[0,0,0,1]]]
else:
print('Error, np.shape of placement is not accepted')
return False
else:
print('Error format of placement is not accepted')
return False
self.viewer[name].set_transform(T)
def delete(self,name):
self[name].delete()
def __getitem__(self,name):
return self.viewer[name]
import math
import time
import numpy as np
import pinocchio as pin
import example_robot_data as robex
from tp2.meshcat_viewer_wrapper import *
robot = robex.load('ur5')
NQ = robot.model.nq
NV = robot.model.nv
# Open the viewer
viz = MeshcatVisualizer(robot,url='classical')
# Add a red box in the viewer
ballID = "world/ball"
viz.addSphere( ballID, 0.1, colors.red )
# Place the ball at the position ( 0.5, 0.1, 0.2 )
# The viewer expect position and rotation, apppend the identity quaternion
q_ball = [0.5, 0.1, 0.2, 1, 0, 0, 0]
viz.applyConfiguration( ballID, q_ball )
#
# PICK #############################################################
#
# Configuration for picking the box
q0 = np.zeros(NQ)
q0[0] = -0.375
q0[1] = -1.2
q0[2] = 1.71
q0[3] = -q0[1] - q0[2]
q0[4] = q0[0]
viz.display(q0)
print("The robot is display with end effector on the red box.")
time.sleep(.2)
#
# MOVE 3D #############################################################
#
print("Let's start the movement ...")
q = q0.copy()
viz.display(q0)
# Random velocity of the robot driving the movement
vq = np.array([ 2.,0,0,4.,0,0])
idx = robot.index('wrist_3_joint')
o_eff = robot.placement(q, idx).translation # Position of end-eff wrt world at current configuration
o_ball = q_ball[:3] # Position of ball wrt world
eff_ball = o_ball - o_eff # Position of ball wrt eff
for i in range(50):
# Chose new configuration of the robot
q += vq / 40
q[2] = 1.71 + math.sin(i * 0.05) / 2
# Gets the new position of the ball
o_ball = robot.placement(q, idx) * eff_ball
# Display new configuration for robot and ball
viz.applyConfiguration(ballID, o_ball.tolist()+[1,0,0,0])
viz.display(q)
time.sleep(0.1)
#
# MOVE 6D #############################################################
#
q = q0.copy()
viz.display(q0)
# Add a red box in the viewer
boxID = "world/box"
viz.delete( ballID )
viz.addBox( boxID, [0.1, 0.2, 0.1], colors.magenta )
# Place the box at the position ( 0.5, 0.1, 0.2 )
q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0]
viz.applyConfiguration( 'world/box', q_box )
print("Now moving with a 6D object ... ")
# Random velocity of the robot driving the movement
vq = np.array([ 2.,0,0,4.,0,0])
idx = robot.index('wrist_3_joint')
oMeff = robot.placement(q, idx) # Placement of end-eff wrt world at current configuration
oMbox = pin.XYZQUATToSE3(q_box) # Placement of box wrt world
effMbox = oMeff.inverse() * oMbox # Placement of box wrt eff
for i in range(1000):
# Chose new configuration of the robot
q += vq / 40
q[2] = 1.71 + math.sin(i * 0.05) / 2
# Gets the new position of the box
oMbox = robot.placement(q, idx) * effMbox
# Display new configuration for robot and box
viz.applyConfiguration(boxID, oMbox)
viz.display(q)
time.sleep(0.1)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment