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

[UTILS] change hastags.

parent fea1e38f
# %do_load import
# %jupyter_snippet import
import pinocchio as pin
from utils.meshcat_viewer_wrapper import MeshcatVisualizer
import time
......@@ -7,47 +7,47 @@ 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_load
# %end_jupyter_snippet
plt.ion() # matplotlib with interactive setting
# %do_load robot
# %jupyter_snippet robot
robot = load_ur5_with_obstacles(reduced=True)
# %end_load
# %end_jupyter_snippet
# %do_load viewer
# %jupyter_snippet viewer
viz = MeshcatVisualizer(robot)
viz.display(robot.q0)
# %end_load
# %end_jupyter_snippet
# %do_load target
# %jupyter_snippet target
target = Target(viz,position = np.array([.5,.5]))
# %end_load
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
# %do_load endef
# %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]]
# %end_load
# %end_jupyter_snippet
# %do_load dist
# %jupyter_snippet dist
def dist(q):
'''Return the distance between the end effector end the target (2d).'''
return norm(endef(q)-target.position)
# %end_load
# %end_jupyter_snippet
# %do_load coll
# %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)
# %end_load
# %end_jupyter_snippet
# %do_load qrand
# %jupyter_snippet qrand
def qrand(check=False):
'''
Return a random configuration. If check is True, this
......@@ -56,21 +56,21 @@ def qrand(check=False):
while True:
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_load
# %end_jupyter_snippet
# %do_load colldist
# %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
# %end_load
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
# %do_not_load qrand_target
# %jupyter_snippet qrand_target
# Sample a random free configuration where dist is small enough.
def qrandTarget(threshold=5e-2, display=False):
while True:
......@@ -81,13 +81,13 @@ def qrandTarget(threshold=5e-2, display=False):
if not coll(q) and dist(q)<threshold:
return q
viz.display(qrandTarget())
# %end_load
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
# %do_not_load random_descent
# %jupyter_snippet random_descent
# Random descent: crawling from one free configuration to the target with random
# steps.
def randomDescent(q0 = None):
......@@ -103,13 +103,13 @@ def randomDescent(q0 = None):
time.sleep(5e-3) # ... and sleep for a short while
return hist
randomDescent();
# %end_load
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
# %do_load sample
# %jupyter_snippet sample
def sampleSpace(nbSamples=500):
'''
Sample nbSamples configurations and store them in two lists depending
......@@ -145,14 +145,14 @@ def plotConfigurationSpace(hcol,hfree,markerSize=20):
hcol,hfree = sampleSpace(100)
plotConfigurationSpace(hcol,hfree)
# %end_load
# %end_jupyter_snippet
################################################################################
################################################################################
################################################################################
### Plot random trajectories in the same plot
# %do_not_load traj
# %jupyter_snippet traj
qinit = np.array([-1.1, -3. ])
for i in range(100):
traj = randomDescent(qinit)
......@@ -164,14 +164,14 @@ traj = np.array(traj)
qend = (traj[-1]+np.pi) % (2*np.pi) - np.pi
### Take the entire trajectory it modulo 2 pi
traj += (qend-traj[-1])
# %end_load
# %end_jupyter_snippet
plt.plot(traj[:,0],traj[:,1],'r',lw=5)
################################################################################
################################################################################
################################################################################
# %do_not_load optim
# %jupyter_snippet optim
def cost(q):
'''
Cost function: distance to the target
......@@ -201,9 +201,9 @@ def optimize():
f_ieqcons=constraint,callback=callback,
full_output=1)
optimize()
# %end_load
# %end_jupyter_snippet
# %do_not_load useit
# %jupyter_snippet useit
while True:
res=optimize()
q=res[0]
......@@ -212,4 +212,4 @@ while True:
print('Finally successful!')
break
print("Failed ... let's try again! ")
# %end_load
# %end_jupyter_snippet
......@@ -27,7 +27,7 @@ NV = robot.model.nv
viz = MeshcatVisualizer(robot)
viz.display(robot.q0)
# %do_load 1
# %jupyter_snippet 1
robot.feetIndexes = [robot.model.getFrameId(frameName) for frameName in ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT']]
# --- Add box to represent target
......@@ -73,7 +73,7 @@ def callback(q):
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)
# %end_load
# %end_jupyter_snippet
class FloatingTest(unittest.TestCase):
......
......@@ -35,7 +35,7 @@ print("Let's go to pdes.")
# Add a vizualization for the target
ballID = "world/ball"
viz.addSphere(ballID, .05, 'green')
# %do_not_load 1
# %jupyter_snippet 1
# Add a vizualisation for the tip of the arm.
tipID = "world/blue"
viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5])
......@@ -58,7 +58,7 @@ def callback(q):
target = np.array([0.5, 0.1, 0.2]) # x,y,z
qopt = fmin_bfgs(cost, robot.q0, callback=callback)
# %end_load
# %end_jupyter_snippet
##########################################################################
### This last part is to automatically validate the versions of this example.
......
......@@ -36,7 +36,7 @@ print("Let's go to pdes.")
# Add a vizualization for the target
boxID = "world/box"
viz.addBox(boxID, [.05, .1, .2], [1., .2, .2, .5])
# %do_load 1
# %jupyter_snippet 1
# Add a vizualisation for the tip of the arm.
tipID = "world/blue"
viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5])
......@@ -63,7 +63,7 @@ Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([-0.5, 0.1, 0.2]))
qopt = fmin_bfgs(cost, robot.q0, callback=callback)
print('The robot finally reached effector placement at\n', robot.placement(qopt, 6))
# %end_load
# %end_jupyter_snippet
class InvGeom6DTest(unittest.TestCase):
......
......@@ -6,7 +6,7 @@ No optimization, this file is just an example of how to load the models.
import numpy as np
import pinocchio as pin
from utils.meshcat_viewer_wrapper import MeshcatVisualizer
# %do_load 0
# %jupyter_snippet 0
from utils.load_ur5_parallel import load_ur5_parallel
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
......@@ -14,11 +14,11 @@ robot = load_ur5_parallel()
# Open the viewer
viz = MeshcatVisualizer(robot)
# %end_load
# %end_jupyter_snippet
viz.display(robot.q0)
# %do_load 1
# %jupyter_snippet 1
# 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]
......@@ -26,10 +26,10 @@ 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)
viz.display(robot.q0)
# %end_load
# %end_jupyter_snippet
# The 4 effector placements are computed by:
# %do_load 2
# %jupyter_snippet 2
effector_indexes = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]
robot.framePlacement(robot.q0, effector_indexes[0])
# %end_load
# %end_jupyter_snippet
......@@ -8,16 +8,16 @@ import example_robot_data as robex
from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors
# %do_load 1
# %jupyter_snippet 1
robot = robex.load('ur5')
# %end_load
# %end_jupyter_snippet
NQ = robot.model.nq
NV = robot.model.nv
# Open the viewer
viz = MeshcatVisualizer(robot)
# %do_load 2
# %jupyter_snippet 2
# Add a red box in the viewer
ballID = "world/ball"
viz.addSphere(ballID, 0.1, colors.red)
......@@ -26,14 +26,14 @@ viz.addSphere(ballID, 0.1, colors.red)
# 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)
# %end_load
# %end_jupyter_snippet
#
# PICK #############################################################
#
# Configuration for picking the box
# %do_load 3
# %jupyter_snippet 3
q0 = np.zeros(NQ) # set the correct values here
q0[0] = -0.375
q0[1] = -1.2
......@@ -44,7 +44,7 @@ q0[5] = 0.
viz.display(q0)
q = q0.copy()
# %end_load
# %end_jupyter_snippet
print("The robot is display with end effector on the red ball.")
#
......@@ -53,7 +53,7 @@ print("The robot is display with end effector on the red ball.")
print("Let's start the movement ...")
# %do_not_load 4
# %jupyter_snippet 4
# Random velocity of the robot driving the movement
vq = np.array([2., 0, 0, 4., 0, 0])
......@@ -74,7 +74,7 @@ for i in range(50):
viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])
viz.display(q)
time.sleep(1e-2)
# %end_load
# %end_jupyter_snippet
#
# MOVE 6D #############################################################
......@@ -83,7 +83,7 @@ for i in range(50):
q = q0.copy()
viz.display(q0)
# %do_load 5
# %jupyter_snippet 5
# Add a red box in the viewer
boxID = "world/box"
#viz.delete(ballID)
......@@ -93,9 +93,9 @@ viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta)
q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0]
viz.applyConfiguration(boxID, q_box)
viz.applyConfiguration(ballID, [2,2,2,1,0,0,0])
# %end_load
# %end_jupyter_snippet
# %do_load 6
# %jupyter_snippet 6
q0 = np.zeros(NQ)
q0[0] = -0.375
q0[1] = -1.2
......@@ -105,11 +105,11 @@ q0[4] = q0[0]
viz.display(q0)
q = q0.copy()
# %end_load
# %end_jupyter_snippet
print("Now moving with a 6D object ... ")
# %do_not_load 7
# %jupyter_snippet 7
# Random velocity of the robot driving the movement
vq = np.array([2., 0, 0, 4., 0, 0])
......@@ -130,7 +130,7 @@ for i in range(100):
viz.applyConfiguration(boxID, oMbox)
viz.display(q)
time.sleep(1e-2)
# %end_load
# %end_jupyter_snippet
##########################################################################
### This last part is to automatically validate the versions of this example.
......
......@@ -4,6 +4,8 @@
from pathlib import Path
import json
hashtags = ['jupyter_snippet']
#hashtags = [ 'do_load', 'do_not_load', 'load' ]
def generate(tp_number: int):
tp = Path() / f'tp{tp_number}'
......@@ -21,21 +23,21 @@ def generate(tp_number: int):
dest = None
with filename.open() as f_in:
for line_number, line in enumerate(f_in):
if line.startswith('# %do_load ') or line.startswith('# %do_not_load'):
if any([ line.startswith(f'# %{hashtag}') for hashtag in hashtags ]):
if dest is not None:
raise SyntaxError(f'do_load / do_not_load block open twice at line {line_number + 1}')
raise SyntaxError(f'%{hashtags[0]} block open twice at line {line_number + 1}')
dest = generated / f'{filename.stem}_{line.split()[2]}'
hidden = '%do_not_load' in line
elif line.strip() == '# %end_load':
hidden = False
elif any([ line.strip() == f'# %end_{hashtag}' for hashtag in hashtags ]):
if dest is None:
raise SyntaxError(f'%end_load block before open at line {line_number + 1}')
raise SyntaxError(f'%{hashtags[0]} block before open at line {line_number + 1}')
with dest.open('w') as f_out:
f_out.write(''.join(content))
content = [f'%do_not_load {dest}\n'] if hidden else [f'# %load {dest}\n', '\n'] + content
#content[-1] = content[-1].strip() # TODO
for cell_number, cell in enumerate(cells_copy):
if any(cell_line.endswith(f'load {dest}') for cell_line in cell['source']):
data['cells'][cell_number]['source'] = content
if cell['source'][0].endswith(f'%load {dest}'):
data['cells'][cell_number]['source'] = [f'# %load {dest}\n'] + content
#if f'%do_not_load {dest}' in cell['source'][0]:
# data['cells'][cell_number]['source'] = [f'%do_not_load {dest}\n']
content = []
hidden = False
dest = None
......
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