Commit 7e9e9287 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

yapf

parent 743c5378
......@@ -25,8 +25,4 @@ format:
- test -f /builds/setup.cfg || ln -s /root/setup.cfg /builds
- test -f /builds/.clang-format || ln -s /root/.clang-format /builds
script:
- flake8 utils examples tp2
- nbqa flake8 2_geometry_3d.ipynb
- yapf -dr utils examples tp2
- nbqa yapf -ri 2_geometry_3d.ipynb
- git diff --exit-code
- entrypoint.sh --no-cpp
......@@ -7,34 +7,46 @@ as example. They ***are not*** related to the robotic models.
# %jupyter_snippet import
import numpy as np
from scipy.optimize import fmin_bfgs, fmin_slsqp
# %end_jupyter_snippet
# %jupyter_snippet cost
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)
# %end_jupyter_snippet
# %jupyter_snippet constraints
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])
# %end_jupyter_snippet
# %jupyter_snippet callback
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
# %end_jupyter_snippet
# %jupyter_snippet bfgs
......
......@@ -3,14 +3,17 @@ import numpy as np
from numpy.linalg import norm
from pinocchio import SE3, AngleAxis, Quaternion
def _lerp(p0, p1, t):
return (1 - t) * p0 + t * p1
def slerp(q0, q1, t):
assert (t >= 0 and t <= 1)
a = AngleAxis(q0.inverse() * q1)
return Quaternion(AngleAxis(a.angle * t, a.axis))
def nlerp(q0, q1, t):
q0 = q0.coeffs()
q1 = q1.coeffs()
......@@ -18,6 +21,7 @@ def nlerp(q0, q1, t):
lerp /= norm(lerp)
return Quaternion(lerp[3], *list(lerp[:3]))
q0 = Quaternion(SE3.Random().rotation)
q1 = Quaternion(SE3.Random().rotation)
viz.applyConfiguration('world/box', [0, 0, 0] + list(q0.coeffs()))
......@@ -28,4 +32,3 @@ for t in np.arange(0, 1, .01):
sleep(.01)
sleep(.1)
viz.applyConfiguration('world/box', [0, 0, 0] + list(q1.coeffs()))
from IPython.core import magic_arguments
from IPython.core.magic import line_magic, cell_magic, line_cell_magic, Magics, magics_class
@magics_class
class DoNotLoadMagics(Magics):
forceLoad = False
@line_magic
def do_not_load(self, line):
if DoNotLoadMagics.forceLoad:
get_ipython().run_line_magic('load',line)
get_ipython().run_line_magic('load', line)
@line_magic
def force_load(self,line):
def force_load(self, line):
if line == "" or line == "on" or line == "True" or line == "1":
DoNotLoadMagics.forceLoad = True
print('Force load in ON')
else:
DoNotLoadMagics.forceLoad = False
print('Force load is OFF')
ip = get_ipython()
ip.register_magics(DoNotLoadMagics)
def forceLoad(force=True):
DoNotLoadMagics.forceLoad = force
print('''NB: as for all the tutorials, a magic command %do_not_load is introduced '''
'''to hide the solutions to some questions. Change it for %load if you want to see '''
'''(and execute) the solution.''')
......@@ -3,9 +3,9 @@ import pinocchio as pin
from utils.meshcat_viewer_wrapper import MeshcatVisualizer
import time
import numpy as np
from numpy.linalg import inv,norm,pinv,svd,eig
from scipy.optimize import fmin_bfgs,fmin_slsqp
from utils.load_ur5_with_obstacles import load_ur5_with_obstacles,Target
from numpy.linalg import inv, norm, pinv, svd, eig
from scipy.optimize import fmin_bfgs, fmin_slsqp
from utils.load_ur5_with_obstacles import load_ur5_with_obstacles, Target
import matplotlib.pylab as plt
# %end_jupyter_snippet
plt.ion() # matplotlib with interactive setting
......@@ -20,33 +20,44 @@ viz.display(robot.q0)
# %end_jupyter_snippet
# %jupyter_snippet target
target = Target(viz,position = np.array([.5,.5]))
target = Target(viz, position=np.array([.5, .5]))
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
# %jupyter_snippet endef
def endef(q):
'''Return the 2d position of the end effector.'''
pin.framesForwardKinematics(robot.model,robot.data,q)
return robot.data.oMf[-1].translation[[0,2]]
'''Return the 2d position of the end effector.'''
pin.framesForwardKinematics(robot.model, robot.data, q)
return robot.data.oMf[-1].translation[[0, 2]]
# %end_jupyter_snippet
# %jupyter_snippet dist
def dist(q):
'''Return the distance between the end effector end the target (2d).'''
return norm(endef(q)-target.position)
'''Return the distance between the end effector end the target (2d).'''
return norm(endef(q) - target.position)
# %end_jupyter_snippet
# %jupyter_snippet coll
def coll(q):
'''Return true if in collision, false otherwise.'''
pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
return pin.computeCollisions(robot.collision_model,robot.collision_data,False)
'''Return true if in collision, false otherwise.'''
pin.updateGeometryPlacements(robot.model, robot.data, robot.collision_model, robot.collision_data, q)
return pin.computeCollisions(robot.collision_model, robot.collision_data, False)
# %end_jupyter_snippet
# %jupyter_snippet qrand
def qrand(check=False):
'''
......@@ -54,97 +65,113 @@ def qrand(check=False):
configuration is not is collision
'''
while True:
q = np.random.rand(2)*6.4-3.2 # sample between -3.2 and +3.2.
q = np.random.rand(2) * 6.4 - 3.2 # sample between -3.2 and +3.2.
if not check or not coll(q): return q
# %end_jupyter_snippet
# %jupyter_snippet colldist
def collisionDistance(q):
'''Return the minimal distance between robot and environment. '''
pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
if pin.computeCollisions(robot.collision_model,robot.collision_data,False): 0
idx = pin.computeDistances(robot.collision_model,robot.collision_data)
return robot.collision_data.distanceResults[idx].min_distance
'''Return the minimal distance between robot and environment. '''
pin.updateGeometryPlacements(robot.model, robot.data, robot.collision_model, robot.collision_data, q)
if pin.computeCollisions(robot.collision_model, robot.collision_data, False): 0
idx = pin.computeDistances(robot.collision_model, robot.collision_data)
return robot.collision_data.distanceResults[idx].min_distance
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
# %jupyter_snippet qrand_target
# Sample a random free configuration where dist is small enough.
def qrandTarget(threshold=5e-2, display=False):
while True:
q = qrand()
if display:
viz.display(q)
time.sleep(1e-3)
if not coll(q) and dist(q)<threshold:
return q
while True:
q = qrand()
if display:
viz.display(q)
time.sleep(1e-3)
if not coll(q) and dist(q) < threshold:
return q
viz.display(qrandTarget())
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
# %jupyter_snippet random_descent
# Random descent: crawling from one free configuration to the target with random
# steps.
def randomDescent(q0 = None):
q = qrand(check=True) if q0 is None else q0
hist = [ q.copy() ]
for i in range(100):
dq = qrand()*.1 # Choose a random step ...
qtry = q+dq # ... apply
if dist(q)>dist(q+dq) and not coll(q+dq): # If distance decrease without collision ...
q = q+dq # ... keep the step
hist.append(q.copy()) # ... keep a trace of it
viz.display(q) # ... display it
time.sleep(5e-3) # ... and sleep for a short while
return hist
randomDescent();
def randomDescent(q0=None):
q = qrand(check=True) if q0 is None else q0
hist = [q.copy()]
for i in range(100):
dq = qrand() * .1 # Choose a random step ...
qtry = q + dq # ... apply
if dist(q) > dist(q + dq) and not coll(q + dq): # If distance decrease without collision ...
q = q + dq # ... keep the step
hist.append(q.copy()) # ... keep a trace of it
viz.display(q) # ... display it
time.sleep(5e-3) # ... and sleep for a short while
return hist
randomDescent()
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
# %jupyter_snippet sample
def sampleSpace(nbSamples=500):
'''
'''
Sample nbSamples configurations and store them in two lists depending
if the configuration is in free space (hfree) or in collision (hcol), along
with the distance to the target and the distance to the obstacles.
'''
hcol = []
hfree = []
for i in range(nbSamples):
q = qrand(False)
if not coll(q):
hfree.append( list(q.flat) + [ dist(q), collisionDistance(q) ])
else:
hcol.append( list(q.flat) + [ dist(q), 1e-2 ])
return hcol,hfree
def plotConfigurationSpace(hcol,hfree,markerSize=20):
'''
hcol = []
hfree = []
for i in range(nbSamples):
q = qrand(False)
if not coll(q):
hfree.append(list(q.flat) + [dist(q), collisionDistance(q)])
else:
hcol.append(list(q.flat) + [dist(q), 1e-2])
return hcol, hfree
def plotConfigurationSpace(hcol, hfree, markerSize=20):
'''
Plot 2 "scatter" plots: the first one plot the distance to the target for
each configuration, the second plots the distance to the obstacles (axis q1,q2,
distance in the color space).
'''
htotal = hcol + hfree
h=np.array(htotal)
plt.subplot(2,1,1)
plt.scatter(h[:,0],h[:,1],c=h[:,2],s=markerSize,lw=0)
plt.title("Distance to the target")
plt.colorbar()
plt.subplot(2,1,2)
plt.scatter(h[:,0],h[:,1],c=h[:,3],s=markerSize,lw=0)
plt.title("Distance to the obstacles")
plt.colorbar()
hcol,hfree = sampleSpace(100)
plotConfigurationSpace(hcol,hfree)
htotal = hcol + hfree
h = np.array(htotal)
plt.subplot(2, 1, 1)
plt.scatter(h[:, 0], h[:, 1], c=h[:, 2], s=markerSize, lw=0)
plt.title("Distance to the target")
plt.colorbar()
plt.subplot(2, 1, 2)
plt.scatter(h[:, 0], h[:, 1], c=h[:, 3], s=markerSize, lw=0)
plt.title("Distance to the obstacles")
plt.colorbar()
hcol, hfree = sampleSpace(100)
plotConfigurationSpace(hcol, hfree)
# %end_jupyter_snippet
################################################################################
......@@ -153,62 +180,65 @@ plotConfigurationSpace(hcol,hfree)
### Plot random trajectories in the same plot
# %jupyter_snippet traj
qinit = np.array([-1.1, -3. ])
qinit = np.array([-1.1, -3.])
for i in range(100):
traj = randomDescent(qinit)
if dist(traj[-1])<5e-2:
print('We found a good traj!')
break
traj = randomDescent(qinit)
if dist(traj[-1]) < 5e-2:
print('We found a good traj!')
break
traj = np.array(traj)
### Chose trajectory end to be in [-pi,pi]
qend = (traj[-1]+np.pi) % (2*np.pi) - np.pi
qend = (traj[-1] + np.pi) % (2 * np.pi) - np.pi
### Take the entire trajectory it modulo 2 pi
traj += (qend-traj[-1])
traj += (qend - traj[-1])
# %end_jupyter_snippet
plt.plot(traj[:,0],traj[:,1],'r',lw=5)
plt.plot(traj[:, 0], traj[:, 1], 'r', lw=5)
################################################################################
################################################################################
################################################################################
# %jupyter_snippet optim
def cost(q):
'''
'''
Cost function: distance to the target
'''
return dist(q)**2
return dist(q)**2
def constraint(q):
'''
'''
Constraint function: distance to the obstacle should be positive.
'''
return collisionDistance(q)
return collisionDistance(q)
def callback(q):
'''
'''
At each optimization step, display the robot configuration in gepetto-viewer.
'''
viz.display(q)
time.sleep(.01)
viz.display(q)
time.sleep(.01)
def optimize():
'''
'''
Optimize from an initial random configuration to discover a collision-free
configuration as close as possible to the target.
'''
return fmin_slsqp(x0=qrand(check=True),
func=cost,
f_ieqcons=constraint,callback=callback,
full_output=1)
return fmin_slsqp(x0=qrand(check=True), func=cost, f_ieqcons=constraint, callback=callback, full_output=1)
optimize()
# %end_jupyter_snippet
# %jupyter_snippet useit
while True:
res=optimize()
q=res[0]
res = optimize()
q = res[0]
viz.display(q)
if res[4]=='Optimization terminated successfully' and res[1]<1e-6:
if res[4] == 'Optimization terminated successfully' and res[1] < 1e-6:
print('Finally successful!')
break
print("Failed ... let's try again! ")
......
......@@ -5,47 +5,57 @@ 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
from utils.meshcat_viewer_wrapper import MeshcatVisualizer,translation2d,planar
from numpy.linalg import norm,inv,pinv,svd,eig
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
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])
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 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))
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.applyConfiguration('target',translation2d(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, ))
......@@ -58,20 +68,30 @@ 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)
x0 = np.array([
0.0,
] * 9)
with_bfgs = 0
if with_bfgs:
......
......@@ -6,61 +6,74 @@ scipy BFGS.
# %jupyter_snippet import
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
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
# %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])
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,))
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]))