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

[TP1] I thought I already commited this one, so I guess the previous

"candidate final" was a mistake.
parent 5387de05
This diff is collapsed.
......@@ -6,42 +6,47 @@ are the placement of the 3 bodies of the robot. BFGS and SLSQP solvers are used.
import time
import numpy as np
from scipy.optimize import fmin_bfgs,fmin_slsqp
import meshcat
from utils.meshcat_viewer_wrapper import MeshcatVisualizer,translation2d,planar
from numpy.linalg import norm,inv,pinv,svd,eig
from meshcat.geometry import Cylinder,Box,Sphere
from transfo import t2d,translation
import colors
viz = meshcat.Visualizer()
viz['joint1'].set_object(Sphere(.1),colors.red)
viz['joint2'].set_object(Sphere(.1),colors.red)
viz['joint3'].set_object(Sphere(.1),colors.red)
viz['arm1'].set_object(Cylinder(.75,.05),colors.grey)
viz['arm2'].set_object(Cylinder(.75,.05),colors.grey)
viz['target'].set_object(Sphere(.1001),colors.green)
viz = MeshcatVisualizer(url='classical')
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])
# %jupyter_snippet display
def display_9(ps):
'''Display the robot in Gepetto Viewer. '''
'''Display the robot in the Viewer. '''
assert (ps.shape == (9, ))
x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps
viz['joint1'].set_transform(t2d(x1, y1, t1))
viz['arm1' ].set_transform(t2d(x1 + np.cos(t1) / 2, x1 + np.sin(t1) / 2, t1))
viz['joint2'].set_transform(t2d(x2, y2, t2))
viz['arm2' ].set_transform(t2d(x2 + np.cos(t2) / 2, y2 + np.sin(t2) / 2, t2))
viz['joint3'].set_transform(t2d(x3, y3, t3))
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))
# %end_jupyter_snippet
# %jupyter_snippet endeffector
def endeffector_9(ps):
assert (ps.shape == (9, ))
x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps
return np.array([x3, y3])
# %end_jupyter_snippet
target = np.array([.5, .5])
viz['target'].set_transform(translation(0,target[0],target[1]))
viz.applyConfiguration('target',translation2d(target[0],target[1]))
# %jupyter_snippet cost
def cost_9(ps):
eff = endeffector_9(ps)
return norm(eff - target)**2
# %end_jupyter_snippet
# %jupyter_snippet constraint
def constraint_9(ps):
assert (ps.shape == (9, ))
x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps
......@@ -53,13 +58,18 @@ def constraint_9(ps):
res[4] = x2 + np.cos(t2) - x3
res[5] = y2 + np.sin(t2) - y3
return res
# %end_jupyter_snippet
# %jupyter_snippet penalty
def penalty(ps):
return cost_9(ps) + 10 * sum(np.square(constraint_9(ps)))
# %end_jupyter_snippet
# %jupyter_snippet callback
def callback_9(ps):
display_9(ps)
time.sleep(.5)
# %end_jupyter_snippet
x0 = np.array([ 0.0,] * 9)
......
......@@ -3,23 +3,26 @@ Stand-alone program to optimize the configuration q=[q1,q2] of a 2-R robot with
scipy BFGS.
'''
# %jupyter_snippet import
import time
import numpy as np
from scipy.optimize import fmin_bfgs
import meshcat
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
from meshcat.geometry import Cylinder,Box,Sphere
from transfo import t2d,translation
import colors
viz = meshcat.Visualizer()
viz['joint1'].set_object(Sphere(.1),colors.red)
viz['joint2'].set_object(Sphere(.1),colors.red)
viz['joint3'].set_object(Sphere(.1),colors.red)
viz['arm1'].set_object(Cylinder(.75,.05),colors.grey)
viz['arm2'].set_object(Cylinder(.75,.05),colors.grey)
viz['target'].set_object(Sphere(.1001),colors.green)
# %end_jupyter_snippet
viz = MeshcatVisualizer(url='classical')
# %jupyter_snippet create
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])
# %end_jupyter_snippet
# %jupyter_snippet display
def display(q):
'''Display the robot in Gepetto Viewer. '''
assert (q.shape == (2,))
......@@ -27,12 +30,14 @@ def display(q):
s0 = np.sin(q[0])
c1 = np.cos(q[0] + q[1])
s1 = np.sin(q[0] + q[1])
viz['joint1'].set_transform(t2d(0, 0, 0))
viz['arm1' ].set_transform(t2d(c0 / 2, s0 / 2, q[0]))
viz['joint2'].set_transform(t2d(c0, s0, q[0]))
viz['arm2' ].set_transform(t2d(c0 + c1 / 2, s0 + s1 / 2, q[0] + q[1]))
viz['joint3'].set_transform(t2d(c0 + c1, s0 + s1, 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]))
# %end_jupyter_snippet
# %jupyter_snippet endeffector
def endeffector(q):
'''Return the 2D position of the end effector of the robot at configuration q. '''
assert (q.shape == (2,))
......@@ -41,18 +46,25 @@ def endeffector(q):
c1 = np.cos(q[0] + q[1])
s1 = np.sin(q[0] + q[1])
return np.array([c0 + c1, s0 + s1])
# %end_jupyter_snippet
# %jupyter_snippet cost
target = np.array([.5, .5])
viz['target'].set_transform(translation(0,target[0],target[1]))
viz.applyConfiguration('target',translation2d(target[0],target[1]))
def cost(q):
eff = endeffector(q)
return np.linalg.norm(eff - target)**2
# %end_jupyter_snippet
# %jupyter_snippet callback
def callback(q):
display(q)
time.sleep(.5)
# %end_jupyter_snippet
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)
# %jupyter_snippet optim
q0 = np.array([0.0, 0.0])
qopt_bfgs = fmin_bfgs(cost, q0, callback=callback)
print('\n *** Optimal configuration from BFGS = %s \n\n\n\n' % qopt_bfgs)
# %end_jupyter_snippet
'''
Example of use a the optimization toolbox of SciPy.
The function optimized here are meaningless, and just given
as example. They ***are not*** related to the robotic models.
'''
import numpy as np
from scipy.optimize import fmin_bfgs, fmin_slsqp
def cost(x):
'''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x '''
x0 = x[0]
x1 = x[1]
return -1 * (2 * x0 * x1 + 2 * x0 - x0**2 - 2 * x1**2)
def constraint_eq(x):
''' Constraint x^3 = y '''
return np.array([x[0]**3 - x[1]])
def constraint_ineq(x):
'''Constraint x>=2, y>=2'''
return np.array([x[0] - 2, x[1] - 2])
class CallbackLogger:
def __init__(self):
self.nfeval = 1
def __call__(self, x):
print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))
self.nfeval += 1
x0 = np.array([0.0, 0.0])
# Optimize cost without any constraints in BFGS, with traces.
xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger())
print('\n *** Xopt in BFGS = %s \n\n\n\n' % str(xopt_bfgs))
# Optimize cost without any constraints in CLSQ
xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1)
print('\n *** Xopt in LSQ = %s \n\n\n\n' % str(xopt_lsq))
# Optimize cost with equality and inequality constraints in CLSQ
xopt_clsq = fmin_slsqp(cost, [-1.0, 1.0], f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, iprint=2, full_output=1)
print('\n *** Xopt in c-lsq = %s \n\n\n\n' % str(xopt_clsq))
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)
q0 = np.array([0.0, 0.0])
qopt_bfgs = fmin_bfgs(cost, q0, callback=callback)
print('\n *** Optimal configuration from BFGS = %s \n\n\n\n' % qopt_bfgs)
from .visualizer import MeshcatVisualizer # noqa
from .transformations import planar,translation2d
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