Commit d96e350e authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[TP1] Candidate final for 2022.

parent abb4be3b
import meshcat
def rgb2int(r,g,b):
return 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)
def callback_9(ps):
display_9(ps)
time.sleep(.5)
def constraint_9(ps):
assert (ps.shape == (9, ))
x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps
res = np.zeros(6)
res[0] = x1 - 0
res[1] = y1 - 0
res[2] = x1 + np.cos(t1) - x2
res[3] = y1 + np.sin(t1) - y2
res[4] = x2 + np.cos(t2) - x3
res[5] = y2 + np.sin(t2) - y3
return res
def cost_9(ps):
eff = endeffector_9(ps)
return norm(eff - target)**2
def display_9(ps):
'''Display the robot in the Viewer. '''
assert (ps.shape == (9, ))
x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps
viz.applyConfiguration('joint1',planar(x1, y1, t1))
viz.applyConfiguration('arm1' ,planar(x1 + np.cos(t1) / 2, x1 + np.sin(t1) / 2, t1))
viz.applyConfiguration('joint2',planar(x2, y2, t2))
viz.applyConfiguration('arm2' ,planar(x2 + np.cos(t2) / 2, y2 + np.sin(t2) / 2, t2))
viz.applyConfiguration('joint3',planar(x3, y3, t3))
def endeffector_9(ps):
assert (ps.shape == (9, ))
x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps
return np.array([x3, y3])
def penalty(ps):
return cost_9(ps) + 10 * sum(np.square(constraint_9(ps)))
def callback(q):
display(q)
time.sleep(.5)
target = np.array([.5, .5])
viz.applyConfiguration('target',translation2d(target[0],target[1]))
def cost(q):
eff = endeffector(q)
return np.linalg.norm(eff - target)**2
viz.addSphere('joint1',.1,[1,0,0,1])
viz.addSphere('joint2',.1,[1,0,0,1])
viz.addSphere('joint3',.1,[1,0,0,1])
viz.addCylinder('arm1',.75,.05,[.65,.65,.65,1])
viz.addCylinder('arm2',.75,.05,[.65,.65,.65,1])
viz.addSphere('target',.1001,[0,.8,.1,1])
def display(q):
'''Display the robot in Gepetto Viewer. '''
assert (q.shape == (2,))
c0 = np.cos(q[0])
s0 = np.sin(q[0])
c1 = np.cos(q[0] + q[1])
s1 = np.sin(q[0] + q[1])
viz.applyConfiguration('joint1',planar(0, 0, 0))
viz.applyConfiguration('arm1' ,planar(c0 / 2, s0 / 2, q[0]))
viz.applyConfiguration('joint2',planar(c0, s0, q[0]))
viz.applyConfiguration('arm2' ,planar(c0 + c1 / 2, s0 + s1 / 2, q[0] + q[1]))
viz.applyConfiguration('joint3',planar(c0 + c1, s0 + s1, q[0] + q[1]))
def endeffector(q):
'''Return the 2D position of the end effector of the robot at configuration q. '''
assert (q.shape == (2,))
c0 = np.cos(q[0])
s0 = np.sin(q[0])
c1 = np.cos(q[0] + q[1])
s1 = np.sin(q[0] + q[1])
return np.array([c0 + c1, s0 + s1])
import time
import numpy as np
from scipy.optimize import fmin_bfgs,fmin_slsqp
from utils.meshcat_viewer_wrapper import MeshcatVisualizer,translation2d,planar
from numpy.linalg import norm,inv,pinv,svd,eig
x0 = np.array([0.0, 0.0])
xopt_bfgs = fmin_bfgs(cost, x0, callback=callback)
print('\n *** Xopt in BFGS = %s \n\n\n\n' % xopt_bfgs)
import numpy as np
import pinocchio as pin
def t3d(x,y,z,qx,qy,qz,qw):
'''Convert a 7d vector (translation-quaternion) into a 4x4 homogeneous matrix as expected by MeshCat. '''
T = np.eye(4)
T[0,3] = x
T[1,3] = y
T[2,3] = z
q = pin.Quaternion(qw,qx,qy,qz)
q.normalize()
T[:3,:3] = q.matrix()
return T
def t2d(x, y, theta):
'''Convert a 3d vector (x,y,theta) into a transformation in the Y,Z plane.'''
s,c=np.sin(theta/2),np.cos(theta / 2)
return t3d(0, x, y, s,0,0,c) # Rotation around X
def translation(x,y,z):
'''Convert a 3d vector (x,y,z) into a pure translation. '''
T = np.eye(4)
T[0,3] = x
T[1,3] = y
T[2,3] = z
return T
'''
Collection of super simple transformations to ease the use of the viewer.
'''
import numpy as np
def planar(x, y, theta):
'''Convert a 3d vector (x,y,theta) into a transformation in the Y,Z plane.'''
s,c=np.sin(theta/2),np.cos(theta / 2)
return [0, x, y, s,0,0,c] # Rotation around X
def translation2d(x,y):
''' Convert a 2d vector (x,y) into a 3d transformation translating the Y,Z plane. '''
return [0,x,y,1,0,0,0]
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