├── tp2 ├── __init__.py ├── generated │ ├── simple_pick_and_place_1 │ ├── invgeom3d_tipid │ ├── parallel_robots_2 │ ├── simple_pick_and_place_6 │ ├── simple_pick_and_place_3 │ ├── parallel_robots_0 │ ├── simple_pick_and_place_2 │ ├── simple_pick_and_place_5 │ ├── invgeom3d_1 │ ├── parallel_robots_1 │ ├── invgeom6d_1 │ ├── simple_pick_and_place_7 │ ├── simple_pick_and_place_4 │ └── floating_1 ├── tests.py ├── parallel_robots.py ├── invgeom3d.py ├── invgeom6d.py ├── floating.py └── simple_pick_and_place.py ├── tp3 ├── __init__.py ├── generated │ ├── inverse_kinematics_robot │ ├── control_head_robot │ ├── inverse_kinematics_frames │ ├── inverse_kinematics_init │ ├── inverse_kinematics_plot │ ├── control_head_gaze │ ├── inverse_kinematics_import │ ├── inverse_kinematics_goal │ ├── inverse_kinematics_6d_loop │ ├── control_head_gaze_loop │ ├── inverse_kinematics_3d_loop │ └── control_head_multi ├── inverse_kinematics.py ├── control_head.py └── tiago_loader.py ├── tp4 ├── __init__.py ├── generated │ ├── solution_pd_init │ ├── solution_pd_dyninv │ ├── solution_pd_integrate │ ├── solution_pd_mass │ ├── example_qp_solve │ ├── solution_pd_robothand │ ├── solution_pd_trajref │ ├── traj_ref_main │ ├── example_qp_matrices │ ├── solution_pd_hyper │ ├── solution_pd_frictionfall │ ├── solution_pd_freefall │ ├── solution_pd_loop │ └── solution_contact_simulator ├── example_qp.py ├── traj_ref.py ├── solution_pd.py └── collision_wrapper.py ├── utils ├── __init__.py ├── meshcat_viewer_wrapper │ ├── __init__.py │ ├── tests.py │ ├── transformations.py │ ├── colors.py │ └── visualizer.py ├── tests.py ├── datastructures │ ├── storage.py │ ├── tree.py │ ├── pathtree.py │ └── mtree │ │ ├── heap_queue.py │ │ └── functions.py ├── generate.py ├── load_ur5_parallel.py └── load_ur5_with_obstacles.py ├── media └── FK.png ├── tp0 ├── generated │ ├── simple_path_planning_robot │ ├── simple_path_planning_target │ ├── simple_path_planning_viewer │ ├── simple_path_planning_dist │ ├── simple_path_planning_endef │ ├── simple_path_planning_useit │ ├── simple_path_planning_coll │ ├── simple_path_planning_qrand │ ├── simple_path_planning_import │ ├── simple_path_planning_qrand_target │ ├── simple_path_planning_traj │ ├── simple_path_planning_colldist │ ├── simple_path_planning_random_descent │ ├── simple_path_planning_optim │ └── simple_path_planning_sample └── simple_path_planning.py ├── tp1 ├── generated │ ├── configuration_reduced_callback │ ├── configuration_extended_callback │ ├── configuration_extended_cost │ ├── configuration_extended_penalty │ ├── configuration_extended_endeffector │ ├── configuration_reduced_optim │ ├── configuration_reduced_import │ ├── configuration_reduced_cost │ ├── configuration_reduced_create │ ├── configuration_reduced_endeffector │ ├── configuration_extended_constraint │ ├── configuration_extended_display │ └── configuration_reduced_display ├── configuration_reduced.py └── configuration_extended.py ├── appendix ├── generated │ ├── scipy_optim_import │ ├── scipy_optim_cost │ ├── scipy_optim_without │ ├── scipy_optim_bfgs │ ├── scipy_optim_constraints │ ├── scipy_optim_callback │ └── scipy_optim_with ├── solution_lerp.py ├── solution_slerp.py └── scipy_optim.py ├── pinocchio_cheat_sheet.pdf ├── tp5 ├── generated │ ├── unicycle_solutions_termmodel │ └── general_ddp_4stage ├── croco_utils.py ├── unicycle_utils.py ├── unicycle_solutions.py └── arm_example.py ├── _meta.md ├── tp7 ├── flow.py ├── qtable.py ├── discretization.py ├── models │ └── pendulum.py ├── deeptable.py ├── qlearn.py ├── qnetwork.py ├── env_pendulum.py └── env_abstract.py ├── robotics_course_env.yml ├── magic_donotload.py ├── LICENSE ├── README.md ├── .gitignore ├── C_scipy_optimizers.ipynb └── B_quaternions.ipynb /tp2/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tp3/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tp4/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /tp2/generated/simple_pick_and_place_1: -------------------------------------------------------------------------------- 1 | robot = robex.load('ur5') 2 | -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- 1 | from .load_ur5_parallel import load_ur5_parallel 2 | -------------------------------------------------------------------------------- /media/FK.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ymontmarin/_tps_robotique/HEAD/media/FK.png -------------------------------------------------------------------------------- /tp4/generated/solution_pd_init: -------------------------------------------------------------------------------- 1 | q = robot.q0.copy() 2 | vq = np.zeros(robot.model.nv) 3 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_robot: -------------------------------------------------------------------------------- 1 | robot = load_ur5_with_obstacles(reduced=True) 2 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_target: -------------------------------------------------------------------------------- 1 | target = Target(viz,position = np.array([.5,.5])) 2 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_viewer: -------------------------------------------------------------------------------- 1 | viz = MeshcatVisualizer(robot) 2 | viz.display(robot.q0) 3 | -------------------------------------------------------------------------------- /tp4/generated/solution_pd_dyninv: -------------------------------------------------------------------------------- 1 | tauq = np.random.rand(robot.model.nv) 2 | aq = inv(M) @ (tauq - b) 3 | -------------------------------------------------------------------------------- /tp4/generated/solution_pd_integrate: -------------------------------------------------------------------------------- 1 | vq += aq * dt 2 | q = pin.integrate(robot.model, q, vq * dt) 3 | -------------------------------------------------------------------------------- /tp1/generated/configuration_reduced_callback: -------------------------------------------------------------------------------- 1 | def callback(q): 2 | display(q) 3 | time.sleep(.5) 4 | -------------------------------------------------------------------------------- /appendix/generated/scipy_optim_import: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.optimize import fmin_bfgs, fmin_slsqp 3 | -------------------------------------------------------------------------------- /pinocchio_cheat_sheet.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ymontmarin/_tps_robotique/HEAD/pinocchio_cheat_sheet.pdf -------------------------------------------------------------------------------- /tp3/generated/inverse_kinematics_robot: -------------------------------------------------------------------------------- 1 | robot = loadTiago() 2 | viz = MeshcatVisualizer(robot,url='classical') 3 | -------------------------------------------------------------------------------- /tp1/generated/configuration_extended_callback: -------------------------------------------------------------------------------- 1 | def callback_9(ps): 2 | display_9(ps) 3 | time.sleep(.5) 4 | -------------------------------------------------------------------------------- /tp4/generated/solution_pd_mass: -------------------------------------------------------------------------------- 1 | M = pin.crba(robot.model, robot.data, q) 2 | b = pin.nle(robot.model, robot.data, q, vq) 3 | -------------------------------------------------------------------------------- /tp3/generated/control_head_robot: -------------------------------------------------------------------------------- 1 | robot = loadTiago(addGazeFrame=True) 2 | viz = MeshcatVisualizer(robot,url='classical') 3 | -------------------------------------------------------------------------------- /tp1/generated/configuration_extended_cost: -------------------------------------------------------------------------------- 1 | def cost_9(ps): 2 | eff = endeffector_9(ps) 3 | return norm(eff - target)**2 4 | -------------------------------------------------------------------------------- /tp1/generated/configuration_extended_penalty: -------------------------------------------------------------------------------- 1 | def penalty(ps): 2 | return cost_9(ps) + 10 * sum(np.square(constraint_9(ps))) 3 | -------------------------------------------------------------------------------- /tp4/generated/example_qp_solve: -------------------------------------------------------------------------------- 1 | [x,cost,_,niter,lag,iact] = quadprog.solve_qp(A,b,C.T,d) # Notice that C.T is passed instead of C 2 | -------------------------------------------------------------------------------- /tp4/generated/solution_pd_robothand: -------------------------------------------------------------------------------- 1 | robot = RobotHand() 2 | viz = MeshcatVisualizer(robot,url='classical') 3 | viz.display(robot.q0) 4 | -------------------------------------------------------------------------------- /tp3/generated/inverse_kinematics_frames: -------------------------------------------------------------------------------- 1 | IDX_TOOL = robot.model.getFrameId('frametool') 2 | IDX_BASIS = robot.model.getFrameId('framebasis') 3 | -------------------------------------------------------------------------------- /utils/meshcat_viewer_wrapper/__init__.py: -------------------------------------------------------------------------------- 1 | from .visualizer import MeshcatVisualizer # noqa 2 | from .transformations import planar,translation2d 3 | -------------------------------------------------------------------------------- /tp2/generated/invgeom3d_tipid: -------------------------------------------------------------------------------- 1 | # Add a vizualisation for the tip of the arm. 2 | tipID = "world/blue" 3 | viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5]) 4 | -------------------------------------------------------------------------------- /tp2/generated/parallel_robots_2: -------------------------------------------------------------------------------- 1 | effector_indexes = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)] 2 | robot.framePlacement(robot.q0, effector_indexes[0]) 3 | -------------------------------------------------------------------------------- /tp4/generated/solution_pd_trajref: -------------------------------------------------------------------------------- 1 | from tp4.traj_ref import TrajRef 2 | qdes = TrajRef(robot.q0,omega = np.array([0,.1,1,1.5,2.5,-1,-1.5,-2.5,.1,.2,.3,.4,.5,.6]),amplitude=1.5) 3 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_dist: -------------------------------------------------------------------------------- 1 | def dist(q): 2 | '''Return the distance between the end effector end the target (2d).''' 3 | return norm(endef(q)-target.position) 4 | -------------------------------------------------------------------------------- /tp4/generated/traj_ref_main: -------------------------------------------------------------------------------- 1 | qdes = TrajRef(np.array([0,0,0.]),omega = np.array([1,2,3.]),amplitude=1.5) 2 | t = 0.2 3 | print(qdes(t),qdes.velocity(t),qdes.acceleration(t)) 4 | -------------------------------------------------------------------------------- /tp1/generated/configuration_extended_endeffector: -------------------------------------------------------------------------------- 1 | def endeffector_9(ps): 2 | assert (ps.shape == (9, )) 3 | x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps 4 | return np.array([x3, y3]) 5 | -------------------------------------------------------------------------------- /appendix/generated/scipy_optim_cost: -------------------------------------------------------------------------------- 1 | def cost(x): 2 | '''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x ''' 3 | x0 = x[0] 4 | x1 = x[1] 5 | return -1 * (2 * x0 * x1 + 2 * x0 - x0**2 - 2 * x1**2) 6 | -------------------------------------------------------------------------------- /tp4/generated/example_qp_matrices: -------------------------------------------------------------------------------- 1 | A = np.random.rand(5,5)*2-1 2 | A = A @ A.T ### Make it positive symmetric 3 | b = np.random.rand(5) 4 | 5 | C = np.random.rand(10,5) 6 | d = np.random.rand(10) 7 | -------------------------------------------------------------------------------- /tp1/generated/configuration_reduced_optim: -------------------------------------------------------------------------------- 1 | q0 = np.array([0.0, 0.0]) 2 | qopt_bfgs = fmin_bfgs(cost, q0, callback=callback) 3 | print('\n *** Optimal configuration from BFGS = %s \n\n\n\n' % qopt_bfgs) 4 | -------------------------------------------------------------------------------- /tp2/generated/simple_pick_and_place_6: -------------------------------------------------------------------------------- 1 | q0 = np.zeros(NQ) 2 | q0[0] = -0.375 3 | q0[1] = -1.2 4 | q0[2] = 1.71 5 | q0[3] = -q0[1] - q0[2] 6 | q0[4] = q0[0] 7 | 8 | viz.display(q0) 9 | q = q0.copy() 10 | -------------------------------------------------------------------------------- /utils/tests.py: -------------------------------------------------------------------------------- 1 | import doctest 2 | 3 | from utils import load_ur5_parallel 4 | 5 | 6 | def load_tests(loader, tests, pattern): 7 | tests.addTests(doctest.DocTestSuite(load_ur5_parallel)) 8 | return tests 9 | -------------------------------------------------------------------------------- /appendix/generated/scipy_optim_without: -------------------------------------------------------------------------------- 1 | # Optimize cost without any constraints in CLSQ 2 | xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1) 3 | print('\n *** Xopt in LSQ = %s \n\n\n\n' % str(xopt_lsq)) 4 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_endef: -------------------------------------------------------------------------------- 1 | def endef(q): 2 | '''Return the 2d position of the end effector.''' 3 | pin.framesForwardKinematics(robot.model,robot.data,q) 4 | return robot.data.oMf[-1].translation[[0,2]] 5 | -------------------------------------------------------------------------------- /tp3/generated/inverse_kinematics_init: -------------------------------------------------------------------------------- 1 | # Robot initial configuration. 2 | q0 = np.array([ 0. , 0. , 1. , 0. , 0.18, 1.37, -0.24, -0.98, 0.98, 3 | 0. , 0. , 0. , 0. , -0.13, 0. , 0. , 0. , 0. ]) 4 | -------------------------------------------------------------------------------- /tp4/generated/solution_pd_hyper: -------------------------------------------------------------------------------- 1 | # Hyperparameters for the control and the simu 2 | Kp = 50. # proportional gain (P of PD) 3 | Kv = 2 * np.sqrt(Kp) # derivative gain (D of PD) 4 | dt = 1e-3 # simulation timestep 5 | -------------------------------------------------------------------------------- /utils/meshcat_viewer_wrapper/tests.py: -------------------------------------------------------------------------------- 1 | import doctest 2 | 3 | from utils.meshcat_viewer_wrapper import colors 4 | 5 | 6 | def load_tests(loader, tests, pattern): 7 | tests.addTests(doctest.DocTestSuite(colors)) 8 | return tests 9 | -------------------------------------------------------------------------------- /appendix/generated/scipy_optim_bfgs: -------------------------------------------------------------------------------- 1 | x0 = np.array([0.0, 0.0]) 2 | # Optimize cost without any constraints in BFGS, with traces. 3 | xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger()) 4 | print('\n *** Xopt in BFGS = %s \n\n\n\n' % str(xopt_bfgs)) 5 | -------------------------------------------------------------------------------- /appendix/generated/scipy_optim_constraints: -------------------------------------------------------------------------------- 1 | def constraint_eq(x): 2 | ''' Constraint x^3 = y ''' 3 | return np.array([x[0]**3 - x[1]]) 4 | 5 | def constraint_ineq(x): 6 | '''Constraint x>=2, y>=2''' 7 | return np.array([x[0] - 2, x[1] - 2]) 8 | -------------------------------------------------------------------------------- /tp2/generated/simple_pick_and_place_3: -------------------------------------------------------------------------------- 1 | 2 | q0 = np.zeros(NQ) # set the correct values here 3 | q0[0] = -0.375 4 | q0[1] = -1.2 5 | q0[2] = 1.71 6 | q0[3] = -q0[1] - q0[2] 7 | q0[4] = q0[0] 8 | q0[5] = 0. 9 | 10 | viz.display(q0) 11 | q = q0.copy() 12 | -------------------------------------------------------------------------------- /tp1/generated/configuration_reduced_import: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | from scipy.optimize import fmin_bfgs,fmin_slsqp 4 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer,translation2d,planar 5 | from numpy.linalg import norm,inv,pinv,svd,eig 6 | -------------------------------------------------------------------------------- /tp1/generated/configuration_reduced_cost: -------------------------------------------------------------------------------- 1 | target = np.array([.5, .5]) 2 | viz.applyConfiguration('target',translation2d(target[0],target[1])) 3 | 4 | def cost(q): 5 | eff = endeffector(q) 6 | return np.linalg.norm(eff - target)**2 7 | -------------------------------------------------------------------------------- /tp2/generated/parallel_robots_0: -------------------------------------------------------------------------------- 1 | from utils.load_ur5_parallel import load_ur5_parallel 2 | 3 | # Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y. 4 | robot = load_ur5_parallel() 5 | 6 | # Open the viewer 7 | viz = MeshcatVisualizer(robot) 8 | -------------------------------------------------------------------------------- /tp2/tests.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | 4 | def load_tests(loader, tests, pattern): 5 | here = Path(__file__) 6 | names = [f'tp2.{p.stem}' for p in here.parent.glob('*.py') if p != here] 7 | tests.addTests(loader.loadTestsFromNames(names)) 8 | return tests 9 | -------------------------------------------------------------------------------- /appendix/generated/scipy_optim_callback: -------------------------------------------------------------------------------- 1 | class CallbackLogger: 2 | def __init__(self): 3 | self.nfeval = 1 4 | 5 | def __call__(self, x): 6 | print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x))) 7 | self.nfeval += 1 8 | -------------------------------------------------------------------------------- /appendix/generated/scipy_optim_with: -------------------------------------------------------------------------------- 1 | # Optimize cost with equality and inequality constraints in CLSQ 2 | xopt_clsq = fmin_slsqp(cost, [-1.0, 1.0], f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, iprint=2, full_output=1) 3 | print('\n *** Xopt in c-lsq = %s \n\n\n\n' % str(xopt_clsq)) 4 | -------------------------------------------------------------------------------- /tp3/generated/inverse_kinematics_plot: -------------------------------------------------------------------------------- 1 | plt.subplot(211) 2 | plt.plot([ e[:3] for e in herr]) 3 | plt.xlabel('control cycle (iter)') 4 | plt.ylabel('error (m)') 5 | plt.subplot(212) 6 | plt.plot([ e[3:] for e in herr]) 7 | plt.xlabel('control cycle (iter)') 8 | plt.ylabel('error (rad)'); 9 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_useit: -------------------------------------------------------------------------------- 1 | while True: 2 | res=optimize() 3 | q=res[0] 4 | viz.display(q) 5 | if res[4]=='Optimization terminated successfully' and res[1]<1e-6: 6 | print('Finally successful!') 7 | break 8 | print("Failed ... let's try again! ") 9 | -------------------------------------------------------------------------------- /tp3/generated/control_head_gaze: -------------------------------------------------------------------------------- 1 | IDX_GAZE = robot.model.getFrameId('framegaze') 2 | 3 | # Add a small ball as a visual target to be reached by the robot 4 | ball = np.array([ 1.2,0.5,1.1 ]) 5 | viz.addSphere('ball', .05, [ .8,.1,.5, .8] ) 6 | viz.applyConfiguration('ball', list(ball)+[0,0,0,1]) 7 | -------------------------------------------------------------------------------- /appendix/solution_lerp.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | 4 | p0 = np.random.rand(3) 5 | p1 = np.random.rand(3) 6 | 7 | for t in np.arange(0, 1, .01): 8 | p = p0 * (1 - t) + p1 * t 9 | viz.applyConfiguration('world/box', list(p) + list(quat.coeffs())) 10 | time.sleep(.01) 11 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_coll: -------------------------------------------------------------------------------- 1 | def coll(q): 2 | '''Return true if in collision, false otherwise.''' 3 | pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) 4 | return pin.computeCollisions(robot.collision_model,robot.collision_data,False) 5 | -------------------------------------------------------------------------------- /tp3/generated/inverse_kinematics_import: -------------------------------------------------------------------------------- 1 | import pinocchio as pin 2 | import numpy as np 3 | import time 4 | from numpy.linalg import pinv,inv,norm,svd,eig 5 | from tp3.tiago_loader import loadTiago 6 | import matplotlib.pylab as plt; plt.ion() 7 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 8 | -------------------------------------------------------------------------------- /tp1/generated/configuration_reduced_create: -------------------------------------------------------------------------------- 1 | viz.addSphere('joint1',.1,[1,0,0,1]) 2 | viz.addSphere('joint2',.1,[1,0,0,1]) 3 | viz.addSphere('joint3',.1,[1,0,0,1]) 4 | viz.addCylinder('arm1',.75,.05,[.65,.65,.65,1]) 5 | viz.addCylinder('arm2',.75,.05,[.65,.65,.65,1]) 6 | viz.addSphere('target',.1001,[0,.8,.1,1]) 7 | -------------------------------------------------------------------------------- /tp3/generated/inverse_kinematics_goal: -------------------------------------------------------------------------------- 1 | # Goal placement, and integration in the viewer of the goal. 2 | oMgoal = pin.SE3(pin.Quaternion(-0.5, 0.58, -0.39, 0.52).normalized().matrix(), 3 | np.array([1.2, .4, .7])) 4 | viz.addBox('goal', [.1,.1,.1], [ .1,.1,.5, .6] ) 5 | viz.applyConfiguration('goal',oMgoal) 6 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_qrand: -------------------------------------------------------------------------------- 1 | def qrand(check=False): 2 | ''' 3 | Return a random configuration. If check is True, this 4 | configuration is not is collision 5 | ''' 6 | while True: 7 | q = np.random.rand(2)*6.4-3.2 # sample between -3.2 and +3.2. 8 | if not check or not coll(q): return q 9 | -------------------------------------------------------------------------------- /tp1/generated/configuration_reduced_endeffector: -------------------------------------------------------------------------------- 1 | def endeffector(q): 2 | '''Return the 2D position of the end effector of the robot at configuration q. ''' 3 | assert (q.shape == (2,)) 4 | c0 = np.cos(q[0]) 5 | s0 = np.sin(q[0]) 6 | c1 = np.cos(q[0] + q[1]) 7 | s1 = np.sin(q[0] + q[1]) 8 | return np.array([c0 + c1, s0 + s1]) 9 | -------------------------------------------------------------------------------- /tp2/generated/simple_pick_and_place_2: -------------------------------------------------------------------------------- 1 | # Add a red box in the viewer 2 | ballID = "world/ball" 3 | viz.addSphere(ballID, 0.1, colors.red) 4 | 5 | # Place the ball at the position ( 0.5, 0.1, 0.2 ) 6 | # The viewer expect position and rotation, apppend the identity quaternion 7 | q_ball = [0.5, 0.1, 0.2, 1, 0, 0, 0] 8 | viz.applyConfiguration(ballID, q_ball) 9 | -------------------------------------------------------------------------------- /tp5/generated/unicycle_solutions_termmodel: -------------------------------------------------------------------------------- 1 | model_term = crocoddyl.ActionModelUnicycle() 2 | 3 | model_term.costWeights = np.matrix([ 4 | 100, # state weight 5 | 0 # control weight 6 | ]).T 7 | 8 | # Define integral+terminal models 9 | problem = crocoddyl.ShootingProblem(x0, [ model ] * T, model_term) 10 | ddp = crocoddyl.SolverDDP(problem) 11 | -------------------------------------------------------------------------------- /tp2/generated/simple_pick_and_place_5: -------------------------------------------------------------------------------- 1 | # Add a red box in the viewer 2 | boxID = "world/box" 3 | #viz.delete(ballID) 4 | viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta) 5 | 6 | # Place the box at the position (0.5, 0.1, 0.2) 7 | q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0] 8 | viz.applyConfiguration(boxID, q_box) 9 | viz.applyConfiguration(ballID, [2,2,2,1,0,0,0]) 10 | -------------------------------------------------------------------------------- /_meta.md: -------------------------------------------------------------------------------- 1 | ## TODO 2 | - Update from 2022 running TPs 3 | - Clear all output for pushed version 4 | - Add LQR finite horizon 5 | - Add GJK-2D 6 | - Add Other planning algorithm 7 | 8 | ## For deployments 9 | - create virgin repo and copy all the content 10 | - do 11 | ``` 12 | rm [0-9]* 13 | rm -r tp[0-9]* 14 | ``` 15 | - add tpX folder and X_ notebooks incrementaly. 16 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_import: -------------------------------------------------------------------------------- 1 | import pinocchio as pin 2 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 3 | import time 4 | import numpy as np 5 | from numpy.linalg import inv,norm,pinv,svd,eig 6 | from scipy.optimize import fmin_bfgs,fmin_slsqp 7 | from utils.load_ur5_with_obstacles import load_ur5_with_obstacles,Target 8 | import matplotlib.pylab as plt 9 | -------------------------------------------------------------------------------- /tp2/generated/invgeom3d_1: -------------------------------------------------------------------------------- 1 | def cost(q): 2 | '''Compute score from a configuration''' 3 | m = robot.framePlacement(q, 22) 4 | p = m.translation 5 | offset = m.rotation[:, 2] * radius 6 | return norm(p + offset - target)**2 7 | 8 | 9 | def callback(q): 10 | viz.display(q) 11 | time.sleep(1e-2) 12 | 13 | q_touch = fmin_bfgs(cost, robot.q0, callback=callback) -------------------------------------------------------------------------------- /tp2/generated/parallel_robots_1: -------------------------------------------------------------------------------- 1 | # Add a new object featuring the parallel robot tool plate. 2 | [w, h, d] = [0.5, 0.5, 0.005] 3 | color = [red, green, blue, transparency] = [1, 1, 0.78, .3] 4 | viz.addBox('world/robot0/toolplate', [w, h, d], color) 5 | Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75])) 6 | viz.applyConfiguration('world/robot0/toolplate', Mtool) 7 | viz.display(robot.q0) 8 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_qrand_target: -------------------------------------------------------------------------------- 1 | # Sample a random free configuration where dist is small enough. 2 | def qrandTarget(threshold=5e-2, display=False): 3 | while True: 4 | q = qrand() 5 | if display: 6 | viz.display(q) 7 | time.sleep(1e-3) 8 | if not coll(q) and dist(q)dist(q+dq) and not coll(q+dq): # If distance decrease without collision ... 10 | q = q+dq # ... keep the step 11 | hist.append(q.copy()) # ... keep a trace of it 12 | viz.display(q) # ... display it 13 | time.sleep(5e-3) # ... and sleep for a short while 14 | return hist 15 | randomDescent(); 16 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_optim: -------------------------------------------------------------------------------- 1 | def cost(q): 2 | ''' 3 | Cost function: distance to the target 4 | ''' 5 | return dist(q)**2 6 | 7 | def constraint(q): 8 | ''' 9 | Constraint function: distance to the obstacle should be positive. 10 | ''' 11 | return collisionDistance(q) - epsilon 12 | 13 | def callback(q): 14 | ''' 15 | At each optimization step, display the robot configuration in gepetto-viewer. 16 | ''' 17 | viz.display(q) 18 | time.sleep(.01) 19 | 20 | def optimize(): 21 | ''' 22 | Optimize from an initial random configuration to discover a collision-free 23 | configuration as close as possible to the target. 24 | ''' 25 | return fmin_slsqp(x0=qrand(check=True), 26 | func=cost, 27 | f_ieqcons=constraint,callback=callback, 28 | full_output=1) 29 | optimize() 30 | -------------------------------------------------------------------------------- /tp3/generated/control_head_gaze_loop: -------------------------------------------------------------------------------- 1 | q = q0.copy() 2 | herr = [] # Log the value of the error between gaze and ball. 3 | # Loop on an inverse kinematics for 200 iterations. 4 | for i in range(500): # Integrate over 2 second of robot life 5 | 6 | # Run the algorithms that outputs values in robot.data 7 | pin.framesForwardKinematics(robot.model,robot.data,q) 8 | pin.computeJointJacobians(robot.model,robot.data,q) 9 | 10 | # Placement from world frame o to frame f oMgaze 11 | oMgaze = robot.data.oMf[IDX_GAZE] 12 | 13 | # 6D jacobian in local frame 14 | o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:] 15 | 16 | # vector from gaze to ball, in world frame 17 | o_GazeBall = oMgaze.translation-ball 18 | 19 | vq = -pinv(o_Jgaze3) @ o_GazeBall 20 | 21 | q = pin.integrate(robot.model,q, vq * DT) 22 | viz.display(q) 23 | time.sleep(1e-3) 24 | 25 | herr.append(o_GazeBall) 26 | -------------------------------------------------------------------------------- /tp2/generated/simple_pick_and_place_4: -------------------------------------------------------------------------------- 1 | q = q_touch.copy() 2 | vq = np.array([2., 0, 0, 4., 0, 0]) 3 | idx = 6 4 | 5 | oMend = robot.placement(q, idx) 6 | o_end = oMend.translation # Position of end-eff express in world frame 7 | o_ball = q_ball[:3] # Position of ball express in world frame 8 | o_end_ball = o_ball - o_end # Relative position of ball center wrt end effector position, express in world frame 9 | end_ball = oMend.rotation.T @ o_end_ball # Position of ball wrt eff in local coordinate 10 | 11 | for i in range(200): 12 | # Chose new configuration of the robot 13 | q += vq / 40 14 | q[2] = 1.71 + math.sin(i * 0.05) / 2 15 | 16 | # Gets the new position of the ball 17 | oMend = robot.placement(q, idx) 18 | o_ball = oMend * end_ball # Apply oMend to the relative placement of ball 19 | 20 | # Display new configuration for robot and ball 21 | viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0]) 22 | viz.display(q) 23 | time.sleep(1e-2) 24 | -------------------------------------------------------------------------------- /appendix/solution_slerp.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | import numpy as np 3 | from numpy.linalg import norm 4 | from pinocchio import SE3, AngleAxis, Quaternion 5 | 6 | def _lerp(p0, p1, t): 7 | return (1 - t) * p0 + t * p1 8 | 9 | def slerp(q0, q1, t): 10 | assert (t >= 0 and t <= 1) 11 | a = AngleAxis(q0.inverse() * q1) 12 | return Quaternion(AngleAxis(a.angle * t, a.axis)) 13 | 14 | def nlerp(q0, q1, t): 15 | q0 = q0.coeffs() 16 | q1 = q1.coeffs() 17 | lerp = _lerp(q0, q1, t) 18 | lerp /= norm(lerp) 19 | return Quaternion(lerp[3], *list(lerp[:3])) 20 | 21 | q0 = Quaternion(SE3.Random().rotation) 22 | q1 = Quaternion(SE3.Random().rotation) 23 | viz.applyConfiguration('world/box', [0, 0, 0] + list(q0.coeffs())) 24 | sleep(.1) 25 | for t in np.arange(0, 1, .01): 26 | q = nlerp(q0, q1, t) 27 | viz.applyConfiguration('world/box', [0, 0, 0] + list(q.coeffs())) 28 | sleep(.01) 29 | sleep(.1) 30 | viz.applyConfiguration('world/box', [0, 0, 0] + list(q1.coeffs())) 31 | 32 | -------------------------------------------------------------------------------- /tp3/generated/inverse_kinematics_3d_loop: -------------------------------------------------------------------------------- 1 | q = q0.copy() 2 | herr = [] # Log the value of the error between tool and goal. 3 | # Loop on an inverse kinematics for 200 iterations. 4 | for i in range(500): # Integrate over 2 second of robot life 5 | 6 | # Run the algorithms that outputs values in robot.data 7 | pin.framesForwardKinematics(robot.model,robot.data,q) 8 | pin.computeJointJacobians(robot.model,robot.data,q) 9 | 10 | # Placement from world frame o to frame f oMtool 11 | oMtool = robot.data.oMf[IDX_TOOL] 12 | 13 | # 3D jacobian in world frame 14 | o_Jtool3 = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL_WORLD_ALIGNED)[:3,:] 15 | 16 | # vector from tool to goal, in world frame 17 | o_TG = oMtool.translation-oMgoal.translation 18 | 19 | # Control law by least square 20 | vq = -pinv(o_Jtool3)@o_TG 21 | 22 | q = pin.integrate(robot.model,q, vq * DT) 23 | viz.display(q) 24 | time.sleep(1e-3) 25 | 26 | herr.append(o_TG) 27 | -------------------------------------------------------------------------------- /tp4/generated/solution_pd_loop: -------------------------------------------------------------------------------- 1 | hq = [] ### For storing the logs of measured trajectory q 2 | hqdes = [] ### For storing the logs of desired trajectory qdes 3 | for it in range(N_steps): 4 | t = it*dt 5 | 6 | # Compute the model. 7 | M = pin.crba(robot.model, robot.data, q) 8 | b = pin.nle(robot.model, robot.data, q, vq) 9 | 10 | # Compute the PD control. 11 | tauq = -Kp*(q-qdes(t)) - Kv*(vq-qdes.velocity(t)) + alpha * qdes.acceleration(t) 12 | tauq_frottement = -Kf * vq 13 | 14 | # Simulated the resulting acceleration (forward dynamics 15 | aq = inv(M) @ (tauq_frottement + tauq - b) 16 | 17 | # Integrate the acceleration. 18 | vq += aq * dt 19 | q = pin.integrate(robot.model, q, vq * dt) 20 | 21 | # Display every TDISP iterations. 22 | TDISP = 50e-3 # Display every 50ms 23 | if not it % int(TDISP/dt): # Only display once in a while ... 24 | viz.display(q) 25 | time.sleep(TDISP) 26 | 27 | # Log the history. 28 | hq.append(q.copy()) 29 | hqdes.append(qdes.copy()) 30 | -------------------------------------------------------------------------------- /tp4/example_qp.py: -------------------------------------------------------------------------------- 1 | ''' 2 | This program presents a minimal example of using the QP solver quadprog. 3 | sudo apt install robotpkg-py35-quadprog 4 | ''' 5 | 6 | import quadprog 7 | import numpy as np 8 | from numpy.linalg import inv,pinv,norm,eig,svd 9 | 10 | # %jupyter_snippet matrices 11 | A = np.random.rand(5,5)*2-1 12 | A = A @ A.T ### Make it positive symmetric 13 | b = np.random.rand(5) 14 | 15 | C = np.random.rand(10,5) 16 | d = np.random.rand(10) 17 | # %end_jupyter_snippet 18 | 19 | assert(np.all(eig(A)[0]>0)) 20 | 21 | # %jupyter_snippet solve 22 | [x,cost,_,niter,lag,iact] = quadprog.solve_qp(A,b,C.T,d) # Notice that C.T is passed instead of C 23 | # %end_jupyter_snippet 24 | 25 | assert( np.isclose(x@A@x/2-b@x,cost) ) 26 | assert( np.all( (C@x-d)>=-1e-6) ) # Check primal KKT condition 27 | assert( np.all( np.isclose( (C@x-d)[iact-1],0 ) ) ) # Check primal complementarity 28 | assert( np.all( np.isclose( (A@x-b -lag@C),0 ) ) ) # Check dual KKT condition 29 | assert( np.all( np.isclose( lag[[ not i in iact-1 for i in range(len(lag)) ]],0 ) ) ) # Check dual complementarity 30 | -------------------------------------------------------------------------------- /magic_donotload.py: -------------------------------------------------------------------------------- 1 | from IPython.core import magic_arguments 2 | from IPython.core.magic import line_magic, cell_magic, line_cell_magic, Magics, magics_class 3 | 4 | @magics_class 5 | class DoNotLoadMagics(Magics): 6 | forceLoad = False 7 | @line_magic 8 | def do_not_load(self, line): 9 | if DoNotLoadMagics.forceLoad: 10 | get_ipython().run_line_magic('load',line) 11 | 12 | @line_magic 13 | def force_load(self,line): 14 | if line == "" or line == "on" or line == "True" or line == "1": 15 | DoNotLoadMagics.forceLoad = True 16 | print('Force load in ON') 17 | else: 18 | DoNotLoadMagics.forceLoad = False 19 | print('Force load is OFF') 20 | 21 | 22 | ip = get_ipython() 23 | ip.register_magics(DoNotLoadMagics) 24 | 25 | def forceLoad(force=True): 26 | DoNotLoadMagics.forceLoad = force 27 | 28 | print('''NB: as for all the tutorials, a magic command %do_not_load is introduced ''' 29 | '''to hide the solutions to some questions. Change it for %load if you want to see ''' 30 | '''(and execute) the solution.''') 31 | -------------------------------------------------------------------------------- /tp5/unicycle_solutions.py: -------------------------------------------------------------------------------- 1 | import crocoddyl 2 | from unicycle_utils import plotUnicycleSolution 3 | import numpy as np 4 | import matplotlib.pylab as plt; plt.ion() 5 | 6 | ### HYPER PARAMS: horizon and initial state 7 | T = 100 8 | x0 = np.array([-1,-1,1]) 9 | 10 | ### PROBLEM DEFINITION 11 | 12 | model = crocoddyl.ActionModelUnicycle() 13 | # %jupyter_snippet termmodel 14 | model_term = crocoddyl.ActionModelUnicycle() 15 | 16 | model_term.costWeights = np.matrix([ 17 | 100, # state weight 18 | 0 # control weight 19 | ]).T 20 | 21 | # Define integral+terminal models 22 | problem = crocoddyl.ShootingProblem(x0, [ model ] * T, model_term) 23 | ddp = crocoddyl.SolverDDP(problem) 24 | # %end_jupyter_snippet 25 | 26 | # Add solvers for verbosity and plots 27 | ddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()]) 28 | 29 | ### SOLVER PROBLEM 30 | done = ddp.solve() 31 | assert(done) 32 | 33 | ### PLOT 34 | log = ddp.getCallbacks()[0] 35 | crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False) 36 | crocoddyl.plotConvergence(log.costs, log.u_regs, log.x_regs, log.grads, log.stops, log.steps, figIndex=2, show=False) 37 | plotUnicycleSolution(log.xs, figIndex=3, show=True) 38 | 39 | -------------------------------------------------------------------------------- /tp3/generated/control_head_multi: -------------------------------------------------------------------------------- 1 | q = q0.copy() 2 | herr = [] # Log the value of the error between tool and goal. 3 | herr2 = [] # Log the value of the error between gaze and ball. 4 | # Loop on an inverse kinematics for 200 iterations. 5 | for i in range(500): # Integrate over 2 second of robot life 6 | 7 | # Run the algorithms that outputs values in robot.data 8 | pin.framesForwardKinematics(robot.model,robot.data,q) 9 | pin.computeJointJacobians(robot.model,robot.data,q) 10 | 11 | # Tool task 12 | oMtool = robot.data.oMf[IDX_TOOL] 13 | tool_Jtool = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL) 14 | tool_nu = pin.log(oMtool.inverse()*oMgoal).vector 15 | 16 | # Gaze task 17 | oMgaze = robot.data.oMf[IDX_GAZE] 18 | o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:] 19 | o_GazeBall = oMgaze.translation-ball 20 | 21 | vq = pinv(tool_Jtool) @ tool_nu 22 | Ptool = np.eye(robot.nv)-pinv(tool_Jtool) @ tool_Jtool 23 | vq += Ptool @ pinv(o_Jgaze3 @ Ptool) @ (-o_GazeBall - o_Jgaze3 @ vq) 24 | 25 | q = pin.integrate(robot.model,q, vq * DT) 26 | viz.display(q) 27 | time.sleep(1e-3) 28 | 29 | herr.append(o_TG) 30 | herr2.append(o_GazeBall) 31 | -------------------------------------------------------------------------------- /tp4/traj_ref.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class TrajRef: 4 | def __init__(self, q0, omega, amplitude): 5 | self.q0 = q0.copy() 6 | self.omega = omega 7 | self.amplitude = amplitude 8 | self.q = self.q0.copy() 9 | self.vq = self.q0*0 10 | self.aq = self.q0*0 11 | def position(self,t): 12 | '''Compute a reference position for time .''' 13 | self.q.flat[:] = self.q0 14 | self.q.flat[:] += self.amplitude*np.sin(self.omega*t) 15 | return self.q 16 | def velocity(self,t): 17 | '''Compute and return the reference velocity at time . ''' 18 | self.vq.flat[:] = self.omega*self.amplitude*np.cos(self.omega*t) 19 | return self.vq 20 | def acceleration(self,t): 21 | '''Compute and return the reference acceleration at time . ''' 22 | self.aq.flat[:] = -self.omega**2*self.amplitude*np.sin(self.omega*t) 23 | return self.aq 24 | def __call__(self,t): return self.position(t) 25 | def copy(self): 26 | return self.q.copy() 27 | 28 | 29 | if __name__ == "__main__": 30 | # %jupyter_snippet main 31 | qdes = TrajRef(np.array([0,0,0.]),omega = np.array([1,2,3.]),amplitude=1.5) 32 | t = 0.2 33 | print(qdes(t),qdes.velocity(t),qdes.acceleration(t)) 34 | # %end_jupyter_snippet 35 | -------------------------------------------------------------------------------- /tp2/parallel_robots.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Load 4 times the UR5 model, plus a plate object on top of them, to feature a simple parallel robot. 3 | No optimization, this file is just an example of how to load the models. 4 | ''' 5 | 6 | import numpy as np 7 | import pinocchio as pin 8 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 9 | # %jupyter_snippet 0 10 | from utils.load_ur5_parallel import load_ur5_parallel 11 | 12 | # Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y. 13 | robot = load_ur5_parallel() 14 | 15 | # Open the viewer 16 | viz = MeshcatVisualizer(robot) 17 | # %end_jupyter_snippet 18 | 19 | viz.display(robot.q0) 20 | 21 | # %jupyter_snippet 1 22 | # Add a new object featuring the parallel robot tool plate. 23 | [w, h, d] = [0.5, 0.5, 0.005] 24 | color = [red, green, blue, transparency] = [1, 1, 0.78, .3] 25 | viz.addBox('world/robot0/toolplate', [w, h, d], color) 26 | Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75])) 27 | viz.applyConfiguration('world/robot0/toolplate', Mtool) 28 | viz.display(robot.q0) 29 | # %end_jupyter_snippet 30 | 31 | # The 4 effector placements are computed by: 32 | # %jupyter_snippet 2 33 | effector_indexes = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)] 34 | robot.framePlacement(robot.q0, effector_indexes[0]) 35 | # %end_jupyter_snippet 36 | -------------------------------------------------------------------------------- /tp0/generated/simple_path_planning_sample: -------------------------------------------------------------------------------- 1 | def sampleSpace(nbSamples=500): 2 | ''' 3 | Sample nbSamples configurations and store them in two lists depending 4 | if the configuration is in free space (hfree) or in collision (hcol), along 5 | with the distance to the target and the distance to the obstacles. 6 | ''' 7 | hcol = [] 8 | hfree = [] 9 | for i in range(nbSamples): 10 | q = qrand(False) 11 | if not coll(q): 12 | hfree.append( list(q.flat) + [ dist(q), collisionDistance(q) ]) 13 | else: 14 | hcol.append( list(q.flat) + [ dist(q), 1e-2 ]) 15 | return hcol,hfree 16 | 17 | def plotConfigurationSpace(hcol,hfree,markerSize=20): 18 | ''' 19 | Plot 2 "scatter" plots: the first one plot the distance to the target for 20 | each configuration, the second plots the distance to the obstacles (axis q1,q2, 21 | distance in the color space). 22 | ''' 23 | htotal = hcol + hfree 24 | h=np.array(htotal) 25 | plt.subplot(2,1,1) 26 | plt.scatter(h[:,0],h[:,1],c=h[:,2],s=markerSize,lw=0) 27 | plt.title("Distance to the target") 28 | plt.colorbar() 29 | plt.subplot(2,1,2) 30 | plt.scatter(h[:,0],h[:,1],c=h[:,3],s=markerSize,lw=0) 31 | plt.title("Distance to the obstacles") 32 | plt.colorbar() 33 | 34 | hcol,hfree = sampleSpace(100) 35 | plotConfigurationSpace(hcol,hfree) 36 | -------------------------------------------------------------------------------- /utils/datastructures/storage.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pickle as pkl 3 | 4 | 5 | class Storage: 6 | 7 | @classmethod 8 | def load(cls, path): 9 | with open(path, 'rb') as f: 10 | a = pkl.load(f) 11 | 12 | inst = cls(a['N'], a['dim']) 13 | 14 | n = a['n'] 15 | inst.n = n 16 | 17 | inst.data[:n] = a['data'] 18 | 19 | return inst 20 | 21 | def __init__(self, N, dim): 22 | self.N = N 23 | self.dim = dim 24 | self.n = np.intp(0) 25 | self.data = np.zeros((N, dim), dtype=float) 26 | 27 | def add_point(self, p): 28 | assert not self.is_full 29 | self.data[self.n] = p 30 | self.n += 1 31 | return self.n - 1 32 | 33 | def remove_last(self): 34 | assert self.n 35 | self.n -= 1 36 | 37 | def __getitem__(self, idx): 38 | # assert idx < self.n 39 | return self.data[idx] 40 | 41 | def __len__(self): 42 | return self.n 43 | 44 | @property 45 | def ndarray(self): 46 | return self.data[:self.n] 47 | 48 | @property 49 | def is_full(self): 50 | return self.n == self.N 51 | 52 | def save(self, path): 53 | with open(path, 'wb') as f: 54 | pkl.dump({ 55 | 'N': self.N, 56 | 'dim': self.dim, 57 | 'n': self.n, 58 | 'data': self.data[:self.n] 59 | }, f) 60 | -------------------------------------------------------------------------------- /tp2/generated/floating_1: -------------------------------------------------------------------------------- 1 | robot.feetIndexes = [robot.model.getFrameId(frameName) for frameName in ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT']] 2 | 3 | # --- Add box to represent target 4 | colors = ['red', 'blue', 'green', 'magenta'] 5 | for color in colors: 6 | viz.addSphere("world/%s" % color, .05, color) 7 | viz.addSphere("world/%s_des" % color, .05, color) 8 | 9 | # 10 | # OPTIM 6D ######################################################### 11 | # 12 | 13 | targets = [ 14 | np.array([-0.7, -0.2, 1.2]), 15 | np.array([-0.3, 0.5, 0.8]), 16 | np.array([0.3, 0.1, -0.1]), 17 | np.array([0.9, 0.9, 0.5]) 18 | ] 19 | for i in range(4): 20 | targets[i][2] += 1 21 | 22 | 23 | def cost(q): 24 | '''Compute score from a configuration''' 25 | cost = 0. 26 | for i in range(4): 27 | p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation 28 | cost += norm(p_i - targets[i])**2 29 | return cost 30 | 31 | 32 | def callback(q): 33 | viz.applyConfiguration('world/box', Mtarget) 34 | 35 | for i in range(4): 36 | p_i = robot.framePlacement(q, robot.feetIndexes[i]) 37 | viz.applyConfiguration('world/%s' % colors[i], p_i) 38 | viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0]) 39 | 40 | viz.display(q) 41 | time.sleep(1e-2) 42 | 43 | 44 | Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2])) # x,y,z 45 | qopt = fmin_bfgs(cost, robot.q0, callback=callback) 46 | -------------------------------------------------------------------------------- /utils/meshcat_viewer_wrapper/colors.py: -------------------------------------------------------------------------------- 1 | import meshcat 2 | 3 | 4 | def rgb2int(r, g, b): 5 | ''' 6 | Convert 3 integers (chars) 0 <= r, g, b < 256 into one single integer = 256**2*r+256*g+b, as expected by Meshcat. 7 | 8 | >>> rgb2int(0, 0, 0) 9 | 0 10 | >>> rgb2int(0, 0, 255) 11 | 255 12 | >>> rgb2int(0, 255, 0) == 0x00FF00 13 | True 14 | >>> rgb2int(255, 0, 0) == 0xFF0000 15 | True 16 | >>> rgb2int(255, 255, 255) == 0xFFFFFF 17 | True 18 | ''' 19 | return int((r << 16) + (g << 8) + b) 20 | 21 | 22 | def material(color, transparent=False): 23 | mat = meshcat.geometry.MeshPhongMaterial() 24 | mat.color = color 25 | mat.transparent = transparent 26 | return mat 27 | 28 | 29 | red = material(color=rgb2int(255, 0, 0), transparent=False) 30 | blue = material(color=rgb2int(0, 0, 255), transparent=False) 31 | green = material(color=rgb2int(0, 255, 0), transparent=False) 32 | yellow = material(color=rgb2int(255, 255, 0), transparent=False) 33 | magenta = material(color=rgb2int(255, 0, 255), transparent=False) 34 | cyan = material(color=rgb2int(0, 255, 255), transparent=False) 35 | white = material(color=rgb2int(250, 250, 250), transparent=False) 36 | black = material(color=rgb2int(5, 5, 5), transparent=False) 37 | grey = material(color=rgb2int(120, 120, 120), transparent=False) 38 | 39 | colormap = { 40 | 'red': red, 41 | 'blue': blue, 42 | 'green': green, 43 | 'yellow': yellow, 44 | 'magenta': magenta, 45 | 'cyan': cyan, 46 | 'black': black, 47 | 'white': white, 48 | 'grey': grey 49 | } 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Nicolas Mansard 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /utils/datastructures/tree.py: -------------------------------------------------------------------------------- 1 | class NodeBinaryTree: 2 | """ 3 | Abstract tree to implement the classic search 4 | """ 5 | def __init__( 6 | self, parent=None, left=None, right=None 7 | ): 8 | self.parent = parent 9 | self.left = left 10 | self.right = right 11 | 12 | def ascension(self): 13 | yield self 14 | if self.parent is not None: 15 | for e in self.parent.ascension(): 16 | yield e 17 | 18 | def depth_first(self): 19 | yield self 20 | if self.left is not None: 21 | for e in self.left.depth_first(): 22 | yield e 23 | if self.right is not None: 24 | for e in self.right.depth_first(): 25 | yield e 26 | 27 | def _wide_first(self, i=0): 28 | yield self, i 29 | iter_left = ( 30 | iter(self.left._wide_first(i + 1)) 31 | if self.left is not None else None 32 | ) 33 | iter_right = ( 34 | iter(self.right._wide_first(i + 1)) 35 | if self.right is not None else None 36 | ) 37 | i_left, n_left = self.robust_next(iter_left) 38 | i_right, n_right = self.robust_next(iter_right) 39 | while not (i_left is None and i_right is None): 40 | if i_left is not None and (i_right is None or i_left <= i_right): 41 | yield i_left, n_left 42 | i_left, n_left = self.robust_next(iter_left) 43 | else: 44 | yield i_right, n_right 45 | i_right, n_right = self.robust_next(iter_right) 46 | 47 | def wide_first(self): 48 | for _, e in self._wide_first(): 49 | yield e 50 | 51 | @staticmethod 52 | def robust_next(iterator): 53 | if iterator is None: 54 | return (None, None) 55 | return next(iterator, (None, None)) 56 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robotic course, XXXX, 20YY 2 | 3 | This repository contains the exercices for the robotics class at XXXX, 20YY. 4 | The exercices are organized by notebook. Each notebook corresponds to one chapter of the class. 5 | The notebooks are in Python and based on the software [Pinocchio](https://github.com/stack-of-tasks/pinocchio). 6 | 7 | ## Getting started 8 | 9 | ### Clone this repository 10 | 11 | Using Git via SSH: 12 | 13 | ```bash 14 | git clone git@github.com:ymontmarin/XXXX_20YY_tps_robotic.git 15 | ``` 16 | 17 | Or via HTTPS: 18 | 19 | ```bash 20 | git clone https://github.com/ymontmarin/XXXX_20YY_tps_robotic.git 21 | ``` 22 | 23 | ### Install miniconda 24 | 25 | - Linux: https://docs.conda.io/en/latest/miniconda.html 26 | - macOS: https://docs.conda.io/en/latest/miniconda.html 27 | - Windows: https://www.anaconda.com/download/ 28 | 29 | Only a little snippet is applied to your home .bashrc, everything else will be segmented! 30 | 31 | ### Run a notebook 32 | 33 | - Go to your local copy of the repository. 34 | - Open a terminal. 35 | - Create the conda environment: 36 | 37 | ```bash 38 | conda env create -f robotics_course_env.yml 39 | ``` 40 | 41 | From there on, to work on a tutorial notebook, you only need to activate the environment: 42 | 43 | ```bash 44 | conda activate robotics_course 45 | ``` 46 | 47 | Then launch the notebook with: 48 | 49 | ```bash 50 | jupyter-lab 51 | ``` 52 | 53 | The notebook will be accessible from your web browser at [localhost:8888](http://localhost:8888). 54 | 55 | Meshcat visualisation can be access in full page in `localhost:700N/static/` where N denotes the Nth meshcat instance created with the running kernel. 56 | 57 | ## Updating the notebooks 58 | 59 | If the repository changes (for instance when new tutorials are pushed) you will need to update your local copy of it by "pulling" from the repository. To do so, go to the directory containing the tutorials and run: 60 | 61 | ``` 62 | git pull 63 | ``` 64 | -------------------------------------------------------------------------------- /appendix/scipy_optim.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Example of use a the optimization toolbox of SciPy. 3 | The function optimized here are meaningless, and just given 4 | as example. They ***are not*** related to the robotic models. 5 | ''' 6 | 7 | # %jupyter_snippet import 8 | import numpy as np 9 | from scipy.optimize import fmin_bfgs, fmin_slsqp 10 | # %end_jupyter_snippet 11 | 12 | # %jupyter_snippet cost 13 | def cost(x): 14 | '''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x ''' 15 | x0 = x[0] 16 | x1 = x[1] 17 | return -1 * (2 * x0 * x1 + 2 * x0 - x0**2 - 2 * x1**2) 18 | # %end_jupyter_snippet 19 | 20 | # %jupyter_snippet constraints 21 | def constraint_eq(x): 22 | ''' Constraint x^3 = y ''' 23 | return np.array([x[0]**3 - x[1]]) 24 | 25 | def constraint_ineq(x): 26 | '''Constraint x>=2, y>=2''' 27 | return np.array([x[0] - 2, x[1] - 2]) 28 | # %end_jupyter_snippet 29 | 30 | # %jupyter_snippet callback 31 | class CallbackLogger: 32 | def __init__(self): 33 | self.nfeval = 1 34 | 35 | def __call__(self, x): 36 | print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x))) 37 | self.nfeval += 1 38 | # %end_jupyter_snippet 39 | 40 | # %jupyter_snippet bfgs 41 | x0 = np.array([0.0, 0.0]) 42 | # Optimize cost without any constraints in BFGS, with traces. 43 | xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger()) 44 | print('\n *** Xopt in BFGS = %s \n\n\n\n' % str(xopt_bfgs)) 45 | # %end_jupyter_snippet 46 | 47 | # %jupyter_snippet without 48 | # Optimize cost without any constraints in CLSQ 49 | xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1) 50 | print('\n *** Xopt in LSQ = %s \n\n\n\n' % str(xopt_lsq)) 51 | # %end_jupyter_snippet 52 | 53 | # %jupyter_snippet with 54 | # Optimize cost with equality and inequality constraints in CLSQ 55 | xopt_clsq = fmin_slsqp(cost, [-1.0, 1.0], f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, iprint=2, full_output=1) 56 | print('\n *** Xopt in c-lsq = %s \n\n\n\n' % str(xopt_clsq)) 57 | # %end_jupyter_snippet 58 | -------------------------------------------------------------------------------- /tp2/invgeom3d.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Stand-alone inverse geom in 3D. Given a reference translation , 3 | it computes the configuration of the UR5 so that the end-effector position (3D) 4 | matches the target. This is done using BFGS solver. While iterating to compute 5 | the optimal configuration, the script also display the successive candidate 6 | solution, hence moving the robot from the initial guess configuaration to the 7 | reference target. 8 | ''' 9 | 10 | import time 11 | import unittest 12 | 13 | import numpy as np 14 | import example_robot_data as robex 15 | from scipy.optimize import fmin_bfgs 16 | from numpy.linalg import norm 17 | 18 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 19 | 20 | # --- Load robot model 21 | robot = robex.load('ur5') 22 | NQ = robot.model.nq 23 | NV = robot.model.nv 24 | 25 | # Open the viewer 26 | viz = MeshcatVisualizer(robot) 27 | 28 | # Define an init config 29 | robot.q0 = np.array([0, -3.14 / 2, 0, 0, 0, 0]) 30 | viz.display(robot.q0) 31 | time.sleep(.3) 32 | print("Let's go to pdes.") 33 | 34 | # --- Add ball to represent target 35 | # Add a vizualization for the target 36 | ballID = "world/ball" 37 | viz.addSphere(ballID, .05, 'green') 38 | # %jupyter_snippet 1 39 | # Add a vizualisation for the tip of the arm. 40 | tipID = "world/blue" 41 | viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5]) 42 | # 43 | # OPTIM 3D ######################################################### 44 | # 45 | 46 | def cost(q): 47 | '''Compute score from a configuration''' 48 | p = robot.placement(q, 6).translation 49 | return norm(p - target)**2 50 | 51 | 52 | def callback(q): 53 | viz.applyConfiguration(ballID, target.tolist() + [0, 1, 0, 0]) 54 | viz.applyConfiguration(tipID, robot.placement(q, 6)) 55 | viz.display(q) 56 | time.sleep(1e-2) 57 | 58 | 59 | target = np.array([0.5, 0.1, 0.2]) # x,y,z 60 | qopt = fmin_bfgs(cost, robot.q0, callback=callback) 61 | # %end_jupyter_snippet 62 | 63 | ########################################################################## 64 | ### This last part is to automatically validate the versions of this example. 65 | class InvGeom3DTest(unittest.TestCase): 66 | def test_qopt_translation(self): 67 | self.assertTrue((np.abs(target - robot.placement(qopt, 6).translation) < 1e-4).all()) 68 | -------------------------------------------------------------------------------- /utils/datastructures/pathtree.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pickle as pkl 3 | from utils.datastructures.storage import Storage 4 | 5 | 6 | class PathTree: 7 | 8 | @classmethod 9 | def load(cls, path): 10 | inst = cls(Storage.load(str(path) + '_storage.pkl')) 11 | with open(str(path) + '_tree.pkl', 'rb') as f: 12 | a = pkl.load(f) 13 | 14 | n = inst.storage.n 15 | inst.parent[:n] = a['parent'] 16 | inst.cost[:n] = a['cost'] 17 | inst.depth[:n] = a['depth'] 18 | 19 | return inst 20 | 21 | def __init__(self, storage): 22 | self.storage = storage 23 | self.parent = np.zeros(storage.N, dtype=np.intp) 24 | self.cost = np.zeros(storage.N, dtype=float) 25 | self.depth = np.zeros(storage.N, dtype=int) 26 | 27 | def update_link(self, q_idx, parent_idx, c=1.): 28 | self.parent[q_idx] = parent_idx 29 | self.depth[q_idx] = self.depth[parent_idx] + 1 30 | self.cost[q_idx] = self.cost[parent_idx] + c 31 | 32 | def get_edges(self): 33 | # TODO use yielding to avoid data overcreation 34 | res = np.zeros((self.storage.n - 1, 2, self.storage.dim), dtype=np.float) 35 | res[:, 0, :] = self.storage.data[1:self.storage.n, :] 36 | res[:, 1, :] = self.storage.data[self.parent[1:self.storage.n], :] 37 | 38 | costs = self.cost[1:self.storage.n] 39 | return res, costs 40 | 41 | def get_path(self): 42 | # TODO use yielding to avoid data overcreation 43 | i = self.storage.n - 1 44 | len_path = self.depth[i] + 1 45 | res = np.zeros((len_path, self.storage.dim)) 46 | j = len_path 47 | while not i == 0: 48 | j -= 1 49 | res[j] = self.storage.data[i] 50 | i = self.parent[i] 51 | res[0] = self.storage.data[0] 52 | return res 53 | 54 | def save(self, path): 55 | n = self.storage.n 56 | self.storage.save(str(path) + '_storage.pkl') 57 | with open(str(path) + '_tree.pkl', 'wb') as f: 58 | pkl.dump({ 59 | 'parent': self.parent[:n], 60 | 'cost': self.cost[:n], 61 | 'depth': self.depth[:n], 62 | }, f) 63 | 64 | def get_estimated_start_goal(self): 65 | return self.storage.data[0], self.storage.data[self.storage.n - 1] 66 | -------------------------------------------------------------------------------- /tp7/qtable.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Example of Q-table learning with a simple discretized 1-pendulum environment. 3 | -- concerge in 1k episods with pendulum(1) 4 | -- Converge in 10k episods with cozmo model 5 | ''' 6 | 7 | import matplotlib.pyplot as plt 8 | import signal 9 | import time 10 | import numpy as np 11 | 12 | ### --- Random seed 13 | RANDOM_SEED = 1188 #int((time.time()%10)*1000) 14 | print("Seed = %d" % RANDOM_SEED) 15 | np.random.seed(RANDOM_SEED) 16 | 17 | ### --- Environment 18 | from tp6.env_pendulum import EnvPendulumDiscrete; Env = lambda : EnvPendulumDiscrete(1,viewer='meshcat') 19 | env = Env() 20 | 21 | ### --- Hyper paramaters 22 | NEPISODES = 400 # Number of training episodes 23 | NSTEPS = 50 # Max episode length 24 | LEARNING_RATE = 0.85 # 25 | DECAY_RATE = 0.99 # Discount factor 26 | 27 | Q = np.zeros([env.nx,env.nu]) # Q-table initialized to 0 28 | 29 | def policy(x): 30 | return np.argmax(Q[x,:]) 31 | 32 | def rendertrial(s0=None,maxiter=100): 33 | '''Roll-out from random state using greedy policy.''' 34 | s = env.reset(s0) 35 | traj = [s] 36 | for i in range(maxiter): 37 | a = np.argmax(Q[s,:]) 38 | s,r = env.step(a) 39 | env.render() 40 | traj.append(s) 41 | return traj 42 | 43 | signal.signal(signal.SIGTSTP, lambda x,y:rendertrial()) # Roll-out when CTRL-Z is pressed 44 | 45 | h_rwd = [] # Learning history (for plot). 46 | for episode in range(1,NEPISODES): 47 | x = env.reset() 48 | rsum = 0.0 49 | for steps in range(NSTEPS): 50 | u = np.argmax(Q[x,:] + np.random.randn(1,env.nu)/episode) # Greedy action with noise 51 | x2,reward = env.step(u) 52 | 53 | # Compute reference Q-value at state x respecting HJB 54 | Qref = reward + DECAY_RATE*np.max(Q[x2,:]) 55 | 56 | # Update Q-Table to better fit HJB 57 | Q[x,u] += LEARNING_RATE*(Qref-Q[x,u]) 58 | x = x2 59 | rsum += reward 60 | 61 | h_rwd.append(rsum) 62 | if not episode%20: 63 | print('Episode #%d done with average cost %.2f' % (episode,sum(h_rwd[-20:])/20)) 64 | 65 | print("Total rate of success: %.3f" % (sum(h_rwd)/NEPISODES)) 66 | rendertrial() 67 | plt.plot( np.cumsum(h_rwd)/range(1,NEPISODES) ) 68 | plt.show() 69 | 70 | -------------------------------------------------------------------------------- /tp2/invgeom6d.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Stand-alone inverse geom in 3D. Given a reference translation , 3 | it computes the configuration of the UR5 so that the end-effector position (3D) 4 | matches the target. This is done using BFGS solver. While iterating to compute 5 | the optimal configuration, the script also display the successive candidate 6 | solution, hence moving the robot from the initial guess configuaration to the 7 | reference target. 8 | ''' 9 | 10 | import time 11 | import unittest 12 | 13 | import numpy as np 14 | import pinocchio as pin 15 | import example_robot_data as robex 16 | from scipy.optimize import fmin_bfgs 17 | from numpy.linalg import norm 18 | 19 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 20 | 21 | # --- Load robot model 22 | robot = robex.load('ur5') 23 | NQ = robot.model.nq 24 | NV = robot.model.nv 25 | 26 | # Open the viewer 27 | viz = MeshcatVisualizer(robot) 28 | 29 | # Define an init config 30 | robot.q0 = np.array([0, -3.14 / 2, 0, 0, 0, 0]) 31 | viz.display(robot.q0) 32 | time.sleep(.3) 33 | print("Let's go to pdes.") 34 | 35 | # --- Add box to represent target 36 | # Add a vizualization for the target 37 | boxID = "world/box" 38 | viz.addBox(boxID, [.05, .1, .2], [1., .2, .2, .5]) 39 | # %jupyter_snippet 1 40 | # Add a vizualisation for the tip of the arm. 41 | tipID = "world/blue" 42 | viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5]) 43 | 44 | # 45 | # OPTIM 6D ######################################################### 46 | # 47 | 48 | 49 | def cost(q): 50 | '''Compute score from a configuration''' 51 | M = robot.placement(q, 6) 52 | return norm(pin.log(M.inverse() * Mtarget).vector) 53 | 54 | 55 | def callback(q): 56 | viz.applyConfiguration(boxID, Mtarget) 57 | viz.applyConfiguration(tipID, robot.placement(q, 6)) 58 | viz.display(q) 59 | time.sleep(1e-2) 60 | 61 | 62 | Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([-0.5, 0.1, 0.2])) # x,y,z 63 | qopt = fmin_bfgs(cost, robot.q0, callback=callback) 64 | 65 | print('The robot finally reached effector placement at\n', robot.placement(qopt, 6)) 66 | # %end_jupyter_snippet 67 | 68 | 69 | class InvGeom6DTest(unittest.TestCase): 70 | def test_qopt_6d(self): 71 | Mopt = robot.placement(qopt, 6) 72 | self.assertTrue((np.abs(Mtarget.translation - Mopt.translation) < 1e-7).all()) 73 | self.assertTrue((np.abs(Mtarget.translation - Mopt.translation) < 1e-7).all()) 74 | -------------------------------------------------------------------------------- /utils/datastructures/mtree/heap_queue.py: -------------------------------------------------------------------------------- 1 | from collections import namedtuple 2 | 3 | 4 | _HeapItem = namedtuple('_HeapItem', 'k, value') 5 | 6 | class HeapQueue(object): 7 | 8 | def __init__(self, content=(), key=lambda x: x, max=False): 9 | if max: 10 | self.key = lambda x: -key(x) 11 | else: 12 | self.key = key 13 | self._items = [_HeapItem(self.key(value), value) for value in content] 14 | self.heapify() 15 | 16 | def _items_less_than(self, base, other): 17 | return self._items[base].k < self._items[other].k 18 | 19 | def _swap_items(self, base, other): 20 | self._items[base], self._items[other] = self._items[other], self._items[base] 21 | 22 | def _make_heap(self, i): 23 | smallest = i 24 | 25 | left = 2 * i + 1 26 | if left < len(self._items) and self._items_less_than(left, smallest): 27 | smallest = left 28 | 29 | right = 2 * i + 2 30 | if right < len(self._items) and self._items_less_than(right, smallest): 31 | smallest = right 32 | 33 | if smallest != i: 34 | self._swap_items(i, smallest) 35 | self._make_heap(smallest) 36 | 37 | def heapify(self): 38 | for i in range(len(self._items) // 2, -1, -1): 39 | self._make_heap(i) 40 | 41 | def head(self): 42 | return self._items[0].value 43 | 44 | def push(self, value): 45 | i = len(self._items) 46 | new_item = _HeapItem(self.key(value), value) 47 | self._items.append(new_item) 48 | while i > 0: 49 | p = int((i - 1) // 2) 50 | if self._items_less_than(p, i): 51 | break 52 | self._swap_items(i, p) 53 | i = p 54 | 55 | def pop(self): 56 | popped = self._items[0].value 57 | self._items[0] = self._items[-1] 58 | self._items.pop(-1) 59 | self._make_heap(0) 60 | return popped 61 | 62 | def pushpop(self, value): 63 | k = self.key(value) 64 | if k <= self._items[0].k: 65 | return value 66 | else: 67 | popped = self._items[0].value 68 | self._items[0] = _HeapItem(k, value) 69 | self._make_heap(0) 70 | return popped 71 | 72 | def __len__(self): 73 | return len(self._items) 74 | 75 | def extractor(self): 76 | while self._items: 77 | yield self.pop() 78 | -------------------------------------------------------------------------------- /tp1/configuration_reduced.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Stand-alone program to optimize the configuration q=[q1,q2] of a 2-R robot with 3 | scipy BFGS. 4 | ''' 5 | 6 | # %jupyter_snippet import 7 | import time 8 | import numpy as np 9 | from scipy.optimize import fmin_bfgs,fmin_slsqp 10 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer,translation2d,planar 11 | from numpy.linalg import norm,inv,pinv,svd,eig 12 | # %end_jupyter_snippet 13 | 14 | viz = MeshcatVisualizer(url='classical') 15 | 16 | # %jupyter_snippet create 17 | viz.addSphere('joint1',.1,[1,0,0,1]) 18 | viz.addSphere('joint2',.1,[1,0,0,1]) 19 | viz.addSphere('joint3',.1,[1,0,0,1]) 20 | viz.addCylinder('arm1',.75,.05,[.65,.65,.65,1]) 21 | viz.addCylinder('arm2',.75,.05,[.65,.65,.65,1]) 22 | viz.addSphere('target',.1001,[0,.8,.1,1]) 23 | # %end_jupyter_snippet 24 | 25 | # %jupyter_snippet display 26 | def display(q): 27 | '''Display the robot in Gepetto Viewer. ''' 28 | assert (q.shape == (2,)) 29 | c0 = np.cos(q[0]) 30 | s0 = np.sin(q[0]) 31 | c1 = np.cos(q[0] + q[1]) 32 | s1 = np.sin(q[0] + q[1]) 33 | viz.applyConfiguration('joint1',planar(0, 0, 0)) 34 | viz.applyConfiguration('arm1' ,planar(c0 / 2, s0 / 2, q[0])) 35 | viz.applyConfiguration('joint2',planar(c0, s0, q[0])) 36 | viz.applyConfiguration('arm2' ,planar(c0 + c1 / 2, s0 + s1 / 2, q[0] + q[1])) 37 | viz.applyConfiguration('joint3',planar(c0 + c1, s0 + s1, q[0] + q[1])) 38 | # %end_jupyter_snippet 39 | 40 | # %jupyter_snippet endeffector 41 | def endeffector(q): 42 | '''Return the 2D position of the end effector of the robot at configuration q. ''' 43 | assert (q.shape == (2,)) 44 | c0 = np.cos(q[0]) 45 | s0 = np.sin(q[0]) 46 | c1 = np.cos(q[0] + q[1]) 47 | s1 = np.sin(q[0] + q[1]) 48 | return np.array([c0 + c1, s0 + s1]) 49 | # %end_jupyter_snippet 50 | 51 | # %jupyter_snippet cost 52 | target = np.array([.5, .5]) 53 | viz.applyConfiguration('target',translation2d(target[0],target[1])) 54 | 55 | def cost(q): 56 | eff = endeffector(q) 57 | return np.linalg.norm(eff - target)**2 58 | # %end_jupyter_snippet 59 | 60 | # %jupyter_snippet callback 61 | def callback(q): 62 | display(q) 63 | time.sleep(.5) 64 | # %end_jupyter_snippet 65 | 66 | # %jupyter_snippet optim 67 | q0 = np.array([0.0, 0.0]) 68 | qopt_bfgs = fmin_bfgs(cost, q0, callback=callback) 69 | print('\n *** Optimal configuration from BFGS = %s \n\n\n\n' % qopt_bfgs) 70 | # %end_jupyter_snippet 71 | -------------------------------------------------------------------------------- /tp2/floating.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Stand-alone inverse geom in 3D. Given a reference translation , 3 | it computes the configuration of the UR5 so that the end-effector position (3D) 4 | matches the target. This is done using BFGS solver. While iterating to compute 5 | the optimal configuration, the script also display the successive candidate 6 | solution, hence moving the robot from the initial guess configuaration to the 7 | reference target. 8 | ''' 9 | 10 | import time 11 | import unittest 12 | 13 | import numpy as np 14 | import pinocchio as pin 15 | import example_robot_data as robex 16 | from scipy.optimize import fmin_bfgs 17 | from numpy.linalg import norm 18 | 19 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 20 | 21 | # --- Load robot model 22 | robot = robex.load('solo12') 23 | NQ = robot.model.nq 24 | NV = robot.model.nv 25 | 26 | # Open the viewer 27 | viz = MeshcatVisualizer(robot) 28 | viz.display(robot.q0) 29 | 30 | # %jupyter_snippet 1 31 | robot.feetIndexes = [robot.model.getFrameId(frameName) for frameName in ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT']] 32 | 33 | # --- Add box to represent target 34 | colors = ['red', 'blue', 'green', 'magenta'] 35 | for color in colors: 36 | viz.addSphere("world/%s" % color, .05, color) 37 | viz.addSphere("world/%s_des" % color, .05, color) 38 | 39 | # 40 | # OPTIM 6D ######################################################### 41 | # 42 | 43 | targets = [ 44 | np.array([-0.7, -0.2, 1.2]), 45 | np.array([-0.3, 0.5, 0.8]), 46 | np.array([0.3, 0.1, -0.1]), 47 | np.array([0.9, 0.9, 0.5]) 48 | ] 49 | for i in range(4): 50 | targets[i][2] += 1 51 | 52 | 53 | def cost(q): 54 | '''Compute score from a configuration''' 55 | cost = 0. 56 | for i in range(4): 57 | p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation 58 | cost += norm(p_i - targets[i])**2 59 | return cost 60 | 61 | 62 | def callback(q): 63 | viz.applyConfiguration('world/box', Mtarget) 64 | 65 | for i in range(4): 66 | p_i = robot.framePlacement(q, robot.feetIndexes[i]) 67 | viz.applyConfiguration('world/%s' % colors[i], p_i) 68 | viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0]) 69 | 70 | viz.display(q) 71 | time.sleep(1e-2) 72 | 73 | 74 | Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2])) # x,y,z 75 | qopt = fmin_bfgs(cost, robot.q0, callback=callback) 76 | # %end_jupyter_snippet 77 | 78 | 79 | class FloatingTest(unittest.TestCase): 80 | def test_cost(self): 81 | self.assertLess(cost(qopt), 1e-10) 82 | -------------------------------------------------------------------------------- /utils/generate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Cut python files in bits loadable by ipython.""" 3 | 4 | from pathlib import Path 5 | import json 6 | 7 | hashtags = ['jupyter_snippet'] 8 | 9 | def generate_from_id(tp_id : int): 10 | folder = Path() / f'tp{tp_id}' 11 | ipynb = next(Path().glob(f'{tp_id}_*.ipynb')) 12 | generate(ipynb,folder) 13 | 14 | def generate(ipynb, folder): 15 | print(f'processing {ipynb} with scripts in {folder}') 16 | with ipynb.open() as f: 17 | data = json.load(f) 18 | cells_copy = data['cells'].copy() 19 | generated = folder / 'generated' 20 | generated.mkdir(exist_ok=True) 21 | for filename in folder.glob('*.py'): 22 | print(f' processing {filename}') 23 | content = [] 24 | hidden = False 25 | dest = None 26 | with filename.open() as f_in: 27 | for line_number, line in enumerate(f_in): 28 | if any([ f'# %{hashtag}' in line for hashtag in hashtags ]): 29 | if dest is not None: 30 | raise SyntaxError(f'%{hashtags[0]} block open twice at line {line_number + 1}') 31 | dest = generated / f'{filename.stem}_{line.split()[2]}' 32 | hidden = False 33 | elif any([ line.strip() == f'# %end_{hashtag}' for hashtag in hashtags ]): 34 | if dest is None: 35 | raise SyntaxError(f'%{hashtags[0]} block before open at line {line_number + 1}') 36 | with dest.open('w') as f_out: 37 | f_out.write(''.join(content)) 38 | for cell_number, cell in enumerate(cells_copy): 39 | if len(cell['source'])==0: continue 40 | if cell['source'][0].endswith(f'%load {dest}'): 41 | data['cells'][cell_number]['source'] = [f'# %load {dest}\n'] + content 42 | #if f'%do_not_load {dest}' in cell['source'][0]: 43 | # data['cells'][cell_number]['source'] = [f'%do_not_load {dest}\n'] 44 | content = [] 45 | hidden = False 46 | dest = None 47 | elif dest is not None: 48 | content.append(line) 49 | with ipynb.open('w') as f: 50 | f.write(json.dumps(data, indent=1)) 51 | 52 | 53 | if __name__ == '__main__': 54 | for tp_number in [0,1,2,3,4,5]: 55 | generate_from_id(tp_number) 56 | 57 | for app in [ 'appendix_scipy_optimizers']: 58 | generate(next(Path().glob(app+'.ipynb')),Path()/'appendix') 59 | -------------------------------------------------------------------------------- /utils/load_ur5_parallel.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio as pin 3 | from example_robot_data import load 4 | 5 | 6 | def load_ur5_parallel(): 7 | """ 8 | Create a robot composed of 4 UR5 9 | 10 | >>> ur5 = load('ur5') 11 | >>> ur5.nq 12 | 6 13 | >>> len(ur5.visual_model.geometryObjects) 14 | 7 15 | >>> robot = load_ur5_parallel() 16 | >>> robot.nq 17 | 24 18 | >>> len(robot.visual_model.geometryObjects) 19 | 28 20 | """ 21 | robot = load('ur5') 22 | nbRobots = 4 23 | 24 | models = [robot.model.copy() for _ in range(nbRobots)] 25 | vmodels = [robot.visual_model.copy() for _ in range(nbRobots)] 26 | 27 | # Build the kinematic model by assembling 4 UR5 28 | fullmodel = pin.Model() 29 | 30 | for irobot, model in enumerate(models): 31 | # Change frame names 32 | for i, f in enumerate(model.frames): 33 | f.name = '%s_#%d' % (f.name, irobot) 34 | # Change joint names 35 | for i, n in enumerate(model.names): 36 | model.names[i] = '%s_#%d' % (n, irobot) 37 | 38 | # Choose the placement of the new arm to be added 39 | Mt = pin.SE3(np.eye(3), np.array([.3, 0, 0.])) # First robot is simply translated 40 | basePlacement = pin.SE3(pin.utils.rotate('z', np.pi * irobot / 2), np.zeros(3)) * Mt 41 | 42 | # Append the kinematic model 43 | fullmodel = pin.appendModel(fullmodel, model, 0, basePlacement) 44 | 45 | # Build the geometry model 46 | fullvmodel = pin.GeometryModel() 47 | 48 | for irobot, (model, vmodel) in enumerate(zip(models, vmodels)): 49 | # Change geometry names 50 | for i, g in enumerate(vmodel.geometryObjects): 51 | # Change the name to avoid conflict 52 | g.name = '%s_#%d' % (g.name, irobot) 53 | 54 | # Refere to new parent names in the full kinematic tree 55 | g.parentFrame = fullmodel.getFrameId(model.frames[g.parentFrame].name) 56 | g.parentJoint = fullmodel.getJointId(model.names[g.parentJoint]) 57 | 58 | # Append the geometry model 59 | fullvmodel.addGeometryObject(g) 60 | # print('add %s on frame %d "%s"' % (g.name, g.parentFrame, fullmodel.frames[g.parentFrame].name)) 61 | 62 | fullrobot = pin.RobotWrapper(fullmodel, fullvmodel, fullvmodel) 63 | # fullrobot.q0 = np.array([-0.375, -1.2 , 1.71 , -0.51 , -0.375, 0. ]*4) 64 | fullrobot.q0 = np.array([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0] * nbRobots) 65 | 66 | return fullrobot 67 | 68 | 69 | if __name__ == "__main__": 70 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 71 | 72 | robot = load_ur5_parallel() 73 | viz = MeshcatVisualizer(robot, url='classical') 74 | viz.display(robot.q0) 75 | -------------------------------------------------------------------------------- /tp4/solution_pd.py: -------------------------------------------------------------------------------- 1 | import pinocchio as pin 2 | import numpy as np 3 | import matplotlib.pylab as plt; plt.ion() 4 | from numpy.linalg import inv, pinv, norm 5 | import time 6 | from tp4.robot_hand import RobotHand 7 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 8 | 9 | # %jupyter_snippet robothand 10 | robot = RobotHand() 11 | viz = MeshcatVisualizer(robot,url='classical') 12 | viz.display(robot.q0) 13 | # %end_jupyter_snippet 14 | 15 | # Initial state position+velocity 16 | # %jupyter_snippet init 17 | q = robot.q0.copy() 18 | vq = np.zeros(robot.model.nv) 19 | # %end_jupyter_snippet 20 | 21 | # %jupyter_snippet hyper 22 | # Hyperparameters for the control and the simu 23 | Kp = 50. # proportional gain (P of PD) 24 | Kv = 2 * np.sqrt(Kp) # derivative gain (D of PD) 25 | dt = 1e-3 # simulation timestep 26 | # %end_jupyter_snippet 27 | 28 | ### Examples for computing the generalized mass matrix and dynamic bias. 29 | # %jupyter_snippet mass 30 | M = pin.crba(robot.model, robot.data, q) 31 | b = pin.nle(robot.model, robot.data, q, vq) 32 | # %end_jupyter_snippet 33 | 34 | ### Example to compute the forward dynamics from M and b 35 | # %jupyter_snippet dyninv 36 | tauq = np.random.rand(robot.model.nv) 37 | aq = inv(M) @ (tauq - b) 38 | # %end_jupyter_snippet 39 | # Alternatively, call the ABA algorithm 40 | aq_bis = pin.aba(robot.model,robot.data,q,vq,tauq) 41 | print(f'Sanity check, should be 0 ... {norm(aq-aq_bis)}') 42 | 43 | ### Example to integrate an acceleration. 44 | # %jupyter_snippet integrate 45 | vq += aq * dt 46 | q = pin.integrate(robot.model, q, vq * dt) 47 | # %end_jupyter_snippet 48 | 49 | ### Reference trajectory 50 | # %jupyter_snippet trajref 51 | from tp4.traj_ref import TrajRef 52 | qdes = TrajRef(robot.q0,omega = np.array([0,.1,1,1.5,2.5,-1,-1.5,-2.5,.1,.2,.3,.4,.5,.6]),amplitude=1.5) 53 | # %end_jupyter_snippet 54 | 55 | # %jupyter_snippet loop 56 | hq = [] ### For storing the logs of measured trajectory q 57 | hqdes = [] ### For storing the logs of desired trajectory qdes 58 | for i in range(10000): 59 | t = i*dt 60 | 61 | # Compute the model. 62 | M = pin.crba(robot.model, robot.data, q) 63 | b = pin.nle(robot.model, robot.data, q, vq) 64 | 65 | # Compute the PD control. 66 | tauq = -Kp*(q-qdes(t)) - Kv*(vq-qdes.velocity(t)) + qdes.acceleration(t) 67 | 68 | # Simulated the resulting acceleration (forward dynamics 69 | aq = inv(M) @ (tauq - b) 70 | 71 | # Integrate the acceleration. 72 | vq += aq * dt 73 | q = pin.integrate(robot.model, q, vq * dt) 74 | 75 | # Display every TDISP iterations. 76 | TDISP = 50e-3 # Display every 50ms 77 | if not i % int(TDISP/dt): # Only display once in a while ... 78 | viz.display(q) 79 | time.sleep(TDISP) 80 | 81 | # Log the history. 82 | hq.append(q.copy()) 83 | hqdes.append(qdes.copy()) 84 | 85 | # %end_jupyter_snippet 86 | -------------------------------------------------------------------------------- /tp1/configuration_extended.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Stand-alone program to optimize the placement of a 2d robot, where the decision variables 3 | are the placement of the 3 bodies of the robot. BFGS and SLSQP solvers are used. 4 | ''' 5 | 6 | import time 7 | import numpy as np 8 | from scipy.optimize import fmin_bfgs,fmin_slsqp 9 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer,translation2d,planar 10 | from numpy.linalg import norm,inv,pinv,svd,eig 11 | 12 | viz = MeshcatVisualizer(url='classical') 13 | 14 | viz.addSphere('joint1',.1,[1,0,0,1]) 15 | viz.addSphere('joint2',.1,[1,0,0,1]) 16 | viz.addSphere('joint3',.1,[1,0,0,1]) 17 | viz.addCylinder('arm1',.75,.05,[.65,.65,.65,1]) 18 | viz.addCylinder('arm2',.75,.05,[.65,.65,.65,1]) 19 | viz.addSphere('target',.1001,[0,.8,.1,1]) 20 | 21 | # %jupyter_snippet display 22 | def display_9(ps): 23 | '''Display the robot in the Viewer. ''' 24 | assert (ps.shape == (9, )) 25 | x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps 26 | viz.applyConfiguration('joint1',planar(x1, y1, t1)) 27 | viz.applyConfiguration('arm1' ,planar(x1 + np.cos(t1) / 2, x1 + np.sin(t1) / 2, t1)) 28 | viz.applyConfiguration('joint2',planar(x2, y2, t2)) 29 | viz.applyConfiguration('arm2' ,planar(x2 + np.cos(t2) / 2, y2 + np.sin(t2) / 2, t2)) 30 | viz.applyConfiguration('joint3',planar(x3, y3, t3)) 31 | # %end_jupyter_snippet 32 | 33 | # %jupyter_snippet endeffector 34 | def endeffector_9(ps): 35 | assert (ps.shape == (9, )) 36 | x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps 37 | return np.array([x3, y3]) 38 | # %end_jupyter_snippet 39 | 40 | target = np.array([.5, .5]) 41 | viz.applyConfiguration('target',translation2d(target[0],target[1])) 42 | 43 | # %jupyter_snippet cost 44 | def cost_9(ps): 45 | eff = endeffector_9(ps) 46 | return norm(eff - target)**2 47 | # %end_jupyter_snippet 48 | 49 | # %jupyter_snippet constraint 50 | def constraint_9(ps): 51 | assert (ps.shape == (9, )) 52 | x1, y1, t1, x2, y2, t2, x3, y3, t3 = ps 53 | res = np.zeros(6) 54 | res[0] = x1 - 0 55 | res[1] = y1 - 0 56 | res[2] = x1 + np.cos(t1) - x2 57 | res[3] = y1 + np.sin(t1) - y2 58 | res[4] = x2 + np.cos(t2) - x3 59 | res[5] = y2 + np.sin(t2) - y3 60 | return res 61 | # %end_jupyter_snippet 62 | 63 | # %jupyter_snippet penalty 64 | def penalty(ps): 65 | return cost_9(ps) + 10 * sum(np.square(constraint_9(ps))) 66 | # %end_jupyter_snippet 67 | 68 | # %jupyter_snippet callback 69 | def callback_9(ps): 70 | display_9(ps) 71 | time.sleep(.5) 72 | # %end_jupyter_snippet 73 | 74 | x0 = np.array([ 0.0,] * 9) 75 | 76 | with_bfgs = 0 77 | if with_bfgs: 78 | xopt = fmin_bfgs(penalty, x0, callback=callback_9) 79 | else: 80 | xopt = fmin_slsqp(cost_9, x0, callback=callback_9, f_eqcons=constraint_9, iprint=2, full_output=1)[0] 81 | print('\n *** Xopt = %s\n\n\n\n' % xopt) 82 | -------------------------------------------------------------------------------- /utils/datastructures/mtree/functions.py: -------------------------------------------------------------------------------- 1 | import random 2 | 3 | from utils.datastructures.mtree.heap_queue import HeapQueue 4 | 5 | 6 | def random_promotion(data_objects, distance_function): 7 | """ 8 | Randomly chooses two objects to be promoted. 9 | """ 10 | data_objects = list(data_objects) 11 | return random.sample(data_objects, 2) 12 | 13 | 14 | def balanced_partition( 15 | promoted_data1, promoted_data2, data_objects, distance_function 16 | ): 17 | partition1 = set() 18 | partition2 = set() 19 | 20 | queue1 = HeapQueue( 21 | data_objects, 22 | key=lambda data: distance_function(data, promoted_data1) 23 | ) 24 | queue2 = HeapQueue( 25 | data_objects, 26 | key=lambda data: distance_function(data, promoted_data2) 27 | ) 28 | 29 | while queue1 or queue2: 30 | while queue1: 31 | data = queue1.pop() 32 | if data not in partition2: 33 | partition1.add(data) 34 | break 35 | 36 | while queue2: 37 | data = queue2.pop() 38 | if data not in partition1: 39 | partition2.add(data) 40 | break 41 | 42 | return partition1, partition2 43 | 44 | 45 | def make_split_function(promotion_function, partition_function): 46 | """ 47 | Creates a splitting function. 48 | The parameters must be callable objects: 49 | - promotion_function(data_objects, distance_function) 50 | Must return two objects chosen from the data_objects argument. 51 | - partition_function(promoted_data1, promoted_data2, data_objects, distance_function) 52 | Must return a sequence with two iterable objects containing a partition 53 | of the data_objects. The promoted_data1 and promoted_data2 arguments 54 | should be used as partitioning criteria and must be contained on the 55 | corresponding iterable subsets. 56 | """ 57 | def split_function(data_objects, distance_function): 58 | promoted_data1, promoted_data2 = promotion_function( 59 | data_objects, distance_function 60 | ) 61 | partition1, partition2 = partition_function( 62 | promoted_data1, promoted_data2, data_objects, distance_function 63 | ) 64 | 65 | return promoted_data1, partition1, promoted_data2, partition2 66 | return split_function 67 | 68 | 69 | def make_cached_distance_function(distance_function): 70 | cache = {} 71 | 72 | def cached_distance_function(data1, data2): 73 | try: 74 | distance = cache[data1][data2] 75 | except KeyError: 76 | distance = distance_function(data1, data2) 77 | 78 | if data1 in cache: 79 | cache[data1][data2] = distance 80 | else: 81 | cache[data1] = {data2: distance} 82 | 83 | if data2 in cache: 84 | cache[data2][data1] = distance 85 | else: 86 | cache[data2] = {data1: distance} 87 | 88 | return distance 89 | 90 | cached_distance_function.cache = cache 91 | 92 | return cached_distance_function 93 | -------------------------------------------------------------------------------- /utils/meshcat_viewer_wrapper/visualizer.py: -------------------------------------------------------------------------------- 1 | import random 2 | 3 | import meshcat 4 | import numpy as np 5 | import pinocchio as pin 6 | from pinocchio.visualize import MeshcatVisualizer as PMV 7 | 8 | from . import colors 9 | 10 | 11 | def materialFromColor(color): 12 | if isinstance(color, meshcat.geometry.MeshPhongMaterial): 13 | return color 14 | elif isinstance(color, str): 15 | material = colors.colormap[color] 16 | elif isinstance(color, list): 17 | material = meshcat.geometry.MeshPhongMaterial() 18 | material.color = colors.rgb2int(*[int(c * 255) for c in color[:3]]) 19 | if len(color) == 3: 20 | material.transparent = False 21 | else: 22 | material.transparent = color[3] < 1 23 | material.opacity = float(color[3]) 24 | elif color is None: 25 | material = random.sample(list(colors.colormap), 1)[0] 26 | else: 27 | material = colors.black 28 | return material 29 | 30 | 31 | class MeshcatVisualizer(PMV): 32 | def __init__(self, robot=None, model=None, collision_model=None, visual_model=None, url=None): 33 | if robot is not None: 34 | super().__init__(robot.model, robot.collision_model, robot.visual_model) 35 | elif model is not None: 36 | super().__init__(model, collision_model, visual_model) 37 | 38 | if url is not None: 39 | if url == 'classical': 40 | url = 'tcp://127.0.0.1:6000' 41 | print('Wrapper tries to connect to server <%s>' % url) 42 | server = meshcat.Visualizer(zmq_url=url) 43 | else: 44 | server = None 45 | 46 | if robot is not None or model is not None: 47 | self.initViewer(loadModel=True, viewer=server) 48 | else: 49 | self.viewer = server if server is not None else meshcat.Visualizer() 50 | 51 | def addSphere(self, name, radius, color): 52 | material = materialFromColor(color) 53 | self.viewer[name].set_object(meshcat.geometry.Sphere(radius), material) 54 | 55 | def addCylinder(self, name, length, radius, color=None): 56 | material = materialFromColor(color) 57 | self.viewer[name].set_object(meshcat.geometry.Cylinder(length, radius), material) 58 | 59 | def addBox(self, name, dims, color): 60 | material = materialFromColor(color) 61 | self.viewer[name].set_object(meshcat.geometry.Box(dims), material) 62 | 63 | def applyConfiguration(self, name, placement): 64 | if isinstance(placement, list) or isinstance(placement, tuple): 65 | placement = np.array(placement) 66 | if isinstance(placement, pin.SE3): 67 | R, p = placement.rotation, placement.translation 68 | T = np.r_[np.c_[R, p], [[0, 0, 0, 1]]] 69 | elif isinstance(placement, np.ndarray): 70 | if placement.shape == (7, ): # XYZ-quat 71 | R = pin.Quaternion(np.reshape(placement[3:], [4, 1])).matrix() 72 | p = placement[:3] 73 | T = np.r_[np.c_[R, p], [[0, 0, 0, 1]]] 74 | else: 75 | print('Error, np.shape of placement is not accepted') 76 | return False 77 | else: 78 | print('Error format of placement is not accepted') 79 | return False 80 | self.viewer[name].set_transform(T) 81 | 82 | def delete(self, name): 83 | self.viewer[name].delete() 84 | 85 | def __getitem__(self, name): 86 | return self.viewer[name] 87 | -------------------------------------------------------------------------------- /tp7/discretization.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Helper class to systematically convert a R^n vector of float into a 3 | N^n vector of integers, and to a single integer. 4 | The main methods are flat2continuous and continuous2flat. 5 | Build it from the size of the real vector space, the bounds in each directions and 6 | the number of discretization steps. 7 | 8 | See also numpy.digitalize. 9 | ''' 10 | 11 | import numpy as np 12 | from functools import reduce 13 | 14 | class VectorDiscretization: 15 | ''' 16 | Convertion between continuous R^N bounded vectors and their discretization 17 | as N^N bounded integer vectors and N*N integer. 18 | ''' 19 | def __init__(self,nv,vmax=1.,vmin=None,nsteps=10,modulo=None,moduloIdx=[]): 20 | self.nv = nv # Dimension of the vector space 21 | self.vmax = vmax if isinstance(vmax,np.ndarray) else np.array([vmax,]*nv) 22 | self.vmin = vmin if isinstance(vmin,np.ndarray) else -self.vmax if vmin is None else np.array([vmin,]*nv) 23 | self.nsteps = nsteps if isinstance(nsteps,np.ndarray) else np.array([nsteps,]*nv) 24 | self.nd = reduce(lambda i,j:i*j,self.nsteps) # prod_{s in nsteps} s = s1*s2*...*s_nv 25 | self.modulo = modulo 26 | self.moduloIdx = moduloIdx 27 | assert( self.nsteps.dtype == np.int32 or self.nsteps.dtype == np.int64) 28 | 29 | def continuous2discrete(self,vc): 30 | if self.modulo is not None and len(self.moduloIdx)>0: 31 | vc = vc.copy() 32 | vc[self.moduloIdx] += self.modulo/2 33 | vc[self.moduloIdx] = vc[self.moduloIdx] % self.modulo 34 | vc[self.moduloIdx] -= self.modulo/2 35 | vc = np.clip(vc,self.vmin,self.vmax) 36 | return ((vc-self.vmin)/(self.vmax-self.vmin)*self.nsteps).astype(self.nsteps.dtype) 37 | 38 | def discrete2continuous(self,vd): 39 | return (vd+.5)*(self.vmax-self.vmin)/self.nsteps+self.vmin 40 | 41 | def flatten(self,vd): 42 | '''change an array of int to a flat int.''' 43 | assert( np.all(vd=0 and i and visuals models for 33 | a N-pendulum. 34 | 35 | @param nbJoint: number of joints of the N-pendulum. 36 | @param length: length of each arm of the pendulum. 37 | @param mass: mass of each arm of the pendulum. 38 | @param viewer: gepetto-viewer CORBA client. If not None, then creates the geometries 39 | in the viewer. 40 | ''' 41 | rmodel = pin.Model() 42 | gmodel = pin.GeometryModel() 43 | 44 | color = [red,green,blue,transparency] = [1,1,0.78,1.0] 45 | colorred = [1.0,0.0,0.0,1.0] 46 | 47 | radius = 0.1*length 48 | 49 | prefix = '' 50 | jointId = 0 51 | jointPlacement = pin.SE3.Identity() 52 | inertia = pin.Inertia(mass, 53 | np.matrix([0.0,0.0,length/2]).T, 54 | mass/5*np.diagflat([ length**2, 1e-2, 1e-2 ]) ) 55 | 56 | for i in range(nbJoint): 57 | # Add a new joint 58 | istr = str(i) 59 | name = prefix+"joint"+istr 60 | jointName,bodyName = [name+"_joint",name+"_body"] 61 | jointId = rmodel.addJoint(jointId,pin.JointModelRX(),jointPlacement,jointName) 62 | rmodel.appendBodyToJoint(jointId,inertia,pin.SE3.Identity()) 63 | jointPlacement = pin.SE3(eye(3),np.matrix([0.0,0.0,length]).T) 64 | 65 | # Add a red sphere for visualizing the joint. 66 | gmodel.addGeometryObject(Capsule(jointName,jointId,1.5*radius,0.0,pin.SE3.Identity(),colorred)) 67 | # Add a white segment for visualizing the link. 68 | gmodel.addGeometryObject(Capsule(bodyName ,jointId,radius,0.8*length, 69 | pin.SE3(eye(3),np.matrix([0.,0.,length/2]).T), 70 | color)) 71 | 72 | rmodel.addFrame( pin.Frame('tip',jointId,0,jointPlacement,pin.FrameType.OP_FRAME) ) 73 | 74 | rmodel.upperPositionLimit = np.zeros(nbJoint)+2*np.pi 75 | rmodel.lowerPositionLimit = np.zeros(nbJoint)-2*np.pi 76 | rmodel.velocityLimit = np.zeros(nbJoint)+5.0 77 | 78 | return rmodel,gmodel 79 | 80 | def createPendulumWrapper(nbJoint,initViewer=True): 81 | ''' 82 | Returns a RobotWrapper with a N-pendulum inside. 83 | ''' 84 | rmodel,gmodel = createPendulum(nbJoint) 85 | rw = pin.RobotWrapper(rmodel,visual_model=gmodel,collision_model=gmodel) 86 | return rw 87 | 88 | if __name__ == "__main__": 89 | rw = createPendulumWrapper(3,True) 90 | rw.initViewer(loadModel=True) 91 | rw.display(np.random.rand(3)*6-3) 92 | 93 | -------------------------------------------------------------------------------- /tp4/generated/solution_contact_simulator: -------------------------------------------------------------------------------- 1 | hq_c = [] ### For storing the logs of measured trajectory q 2 | hqdes_c = [] ### For storing the logs of desired trajectory qdes 3 | 4 | tracked_collisions_id = set() # Track contact 5 | 6 | for it in range(N_steps): 7 | t = it*dt 8 | 9 | # Compute the dynamics quantities for the model 10 | M = pin.crba(robot.model, robot.data, q) # Inertie matrix 11 | b = pin.nle(robot.model, robot.data, q, vq) # Coriolis term 12 | 13 | # Compute the PD torque control we apply in actuators order to follow qdes 14 | tauq_control = - Kp * (q - qdes(t)) - Kv * (vq - qdes.velocity(t)) + alpha * qdes.acceleration(t) 15 | # Compute the friction torque 16 | tauq_friction = - Kf * vq 17 | 18 | # Simulate the resulting free acceleration (forward dynamics) 19 | # The total torque is the control torque and the friction torque 20 | # It corresponds to the acceleration qddot if there is no contact 21 | aq0 = inv(M) @ (tauq_friction + tauq_control - b) 22 | 23 | # Check collision to calculate the real acceleration that respects the contacts 24 | colwrap.computeCollisions(q, vq) 25 | raw_collisions = colwrap.getCollisionList() 26 | raw_dist = colwrap.getCollisionDistances(raw_collisions) 27 | 28 | # Keep only the significative collisions because of numerical errors 29 | collisions = [c for c, d in zip(raw_collisions, raw_dist) if d <= -1e-4] 30 | 31 | if not collisions: 32 | # No real collision so acceleration is the free acceleration 33 | aq = aq0 34 | # There is no contact so we reset the tracker 35 | tracked_collisions_id = set() 36 | else: 37 | # extract dists, J_c et Jdotqdot for the actual collisions 38 | dists = colwrap.getCollisionDistances(collisions) 39 | J = colwrap.getCollisionJacobian(collisions) 40 | JdotQdot = colwrap.getCollisionJdotQdot(collisions) 41 | 42 | # Use the id of collisions to identify the new collisions 43 | # for a collision col, col[0] is a unique identifier 44 | collisions_id = [col[0] for col in collisions] 45 | # get the pyidx (i.e., the python index) for collision with id (i.e. col[0]) not already tracked 46 | new_collisions_pyidx = [ 47 | pyidx 48 | for pyidx, col_id in enumerate(collisions_id) 49 | if col_id not in tracked_collisions_id 50 | ] 51 | # Update our tracker for the next iteration 52 | tracked_collisions_id = set(collisions_id) 53 | 54 | # For the new collisions, adapt vq such that vc is 0 55 | if new_collisions_pyidx: 56 | # We extract the line of J only for the new collisions 57 | J_proj = np.stack([J[i] for i in new_collisions_pyidx], axis=0) 58 | # And we use pinv to project vq in the kernel of J_proj such that vs=0 59 | vq -= (pinv(J_proj) @ J_proj) @ vq 60 | 61 | # Construct the minimization problem associated with the Gauss principle 62 | # The constraint is ac >= PD(e, vc) instead of e >= 0 63 | A = M 64 | b = M @ aq0 65 | C = J 66 | d = - JdotQdot - Kp_c * dists - Kv_c * J @ vq 67 | 68 | [aq,cost,_,niter,lag,iact] = quadprog.solve_qp(A,b,C.T,d) 69 | 70 | # Integrate the acceleration. 71 | vq += aq * dt 72 | q = pin.integrate(robot.model, q, vq * dt) 73 | 74 | # Display every TDISP iterations. 75 | TDISP = 50e-3 # Display every 50ms 76 | if not it % int(TDISP/dt): # Only display once in a while ... 77 | viz.display(q) 78 | time.sleep(TDISP) 79 | 80 | # Log the history. 81 | hq_c.append(q.copy()) 82 | hqdes_c.append(qdes.copy()) 83 | -------------------------------------------------------------------------------- /tp5/generated/general_ddp_4stage: -------------------------------------------------------------------------------- 1 | state = crocoddyl.StateMultibody(robot_model) 2 | # The state object containt info about the configspace, the constraint, the velocity, the kinematic values and everything 3 | 4 | # State is quite abstract, to have a real solvable OCP we need to define somme cost associated to this model 5 | # We also need to decide the actual dynamic we will follow (called actuation), it means a solver that will be ourf(x,u) AND a way to descretize it 6 | 7 | rcms = [] 8 | tcms = [] 9 | 10 | for i in range(4): 11 | runningCostModel = crocoddyl.CostModelSum(state) # The state will remain the same during running 12 | terminalCostModel = crocoddyl.CostModelSum(state) # And also for final state 13 | 14 | ### Cost for reaching the target 15 | pref = crocoddyl.FrameTranslation(FRAME_TIP, goals[i]) 16 | goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, pref) 17 | 18 | ### Cost for regularizing the state about robot_model.x0, running AND terminal 19 | weights=crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1,1,1,1,1, 1,1,1,1,2,2,2.])) 20 | xRegCost = crocoddyl.CostModelState(state,weights,robot_model.x0) 21 | weightsT=crocoddyl.ActivationModelWeightedQuad(np.array([.01,.01,.01,.01,.01,.01,.01, 1,1,1,1,2,2,2.])) 22 | xRegCostT = crocoddyl.CostModelState(state,weightsT,robot_model.x0) 23 | 24 | ### Cost for keeping the control low: around zeros with uniform weights 25 | uRegCost = crocoddyl.CostModelControl(state) 26 | 27 | 28 | runningCostModel.addCost("gripperPose", goalTrackingCost, .001) # addCost is a method of CostModelSum that take another CostModel 29 | runningCostModel.addCost("xReg", xRegCost, 1e-3) # We also weight the sum of cost 30 | runningCostModel.addCost("uReg", uRegCost, 1e-6) 31 | terminalCostModel.addCost("gripperPose", goalTrackingCost, 10) 32 | terminalCostModel.addCost("xReg", xRegCostT, .01) 33 | terminalCostModel.addCost("uReg", xRegCostT, 1e-6) 34 | 35 | rcms.append(runningCostModel) 36 | tcms.append(terminalCostModel) 37 | 38 | 39 | actuationModel = crocoddyl.ActuationModelFull(state) 40 | dt = 1e-2 41 | # A step in the running step 42 | ## We use the freefall forward dynamic as dotx = f(x,u) it could be a contact one (as in prev TP) and so on 43 | ### It is the differential model chosen around the state 44 | ## We precise the duration of the step dt 45 | ## We precise the integration scheme we use 46 | ### It is the integrated model chosen around the differential model 47 | ## We precise which cost is used during this step 48 | def make_running_model(rcm): 49 | runningModel = crocoddyl.IntegratedActionModelEuler( 50 | crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, rcm), dt) 51 | runningModel.differential.armature = robot_model.armature 52 | return runningModel 53 | 54 | def make_terminal_model(tcm): 55 | runningModel = crocoddyl.IntegratedActionModelEuler( 56 | crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, tcm), 0.) 57 | runningModel.differential.armature = robot_model.armature 58 | return runningModel 59 | 60 | terminalModel = crocoddyl.IntegratedActionModelEuler( 61 | crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.) 62 | terminalModel.differential.armature = robot_model.armature 63 | 64 | T = 100 65 | T_inter = 1 66 | 67 | def create_problem(n): 68 | rmseq = [] 69 | for i in range(n - 1): 70 | rmseq += [make_running_model(rcms[i])] * T 71 | rmseq += [make_running_model(tcms[i])] * T_inter 72 | rmseq += [make_running_model(rcms[n-1])] * T 73 | tm = make_terminal_model(tcms[n-1]) 74 | return crocoddyl.ShootingProblem(robot_model.x0, rmseq, tm) 75 | 76 | ddp = crocoddyl.SolverDDP(create_problem(4)) 77 | ddp.solve() 78 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | 162 | -------------------------------------------------------------------------------- /tp3/inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Inverse kinematics (close loop / iterative) for a mobile manipulator. 3 | This basic example simply provides two control loops, to servo -first- the position of the gripper 4 | then -second- the placement (position orientation) of the gripper. 5 | ''' 6 | 7 | # %jupyter_snippet import 8 | import pinocchio as pin 9 | import numpy as np 10 | import time 11 | from numpy.linalg import pinv,inv,norm,svd,eig 12 | from tp3.tiago_loader import loadTiago 13 | import matplotlib.pylab as plt; plt.ion() 14 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 15 | # %end_jupyter_snippet 16 | 17 | # %jupyter_snippet robot 18 | robot = loadTiago() 19 | viz = MeshcatVisualizer(robot,url='classical') 20 | # %end_jupyter_snippet 21 | 22 | NQ = robot.model.nq 23 | NV = robot.model.nv 24 | # %jupyter_snippet frames 25 | IDX_TOOL = robot.model.getFrameId('frametool') 26 | IDX_BASIS = robot.model.getFrameId('framebasis') 27 | # %end_jupyter_snippet 28 | IDX_GAZE = robot.model.getFrameId('framegaze') 29 | 30 | # %jupyter_snippet goal 31 | # Goal placement, and integration in the viewer of the goal. 32 | oMgoal = pin.SE3(pin.Quaternion(-0.5, 0.58, -0.39, 0.52).normalized().matrix(), 33 | np.array([1.2, .4, .7])) 34 | viz.addBox('goal', [.1,.1,.1], [ .1,.1,.5, .6] ) 35 | viz.applyConfiguration('goal',oMgoal) 36 | # %end_jupyter_snippet 37 | 38 | # Integration step. 39 | DT = 1e-2 40 | 41 | # %jupyter_snippet init 42 | # Robot initial configuration. 43 | q0 = np.array([ 0. , 0. , 1. , 0. , 0.18, 1.37, -0.24, -0.98, 0.98, 44 | 0. , 0. , 0. , 0. , -0.13, 0. , 0. , 0. , 0. ]) 45 | # %end_jupyter_snippet 46 | 47 | # %jupyter_snippet 3d_loop 48 | q = q0.copy() 49 | herr = [] # Log the value of the error between tool and goal. 50 | # Loop on an inverse kinematics for 200 iterations. 51 | for i in range(200): # Integrate over 2 second of robot life 52 | 53 | # Run the algorithms that outputs values in robot.data 54 | pin.framesForwardKinematics(robot.model,robot.data,q) 55 | pin.computeJointJacobians(robot.model,robot.data,q) 56 | 57 | # Placement from world frame o to frame f oMtool 58 | oMtool = robot.data.oMf[IDX_TOOL] 59 | 60 | # 3D jacobian in world frame 61 | o_Jtool3 = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL_WORLD_ALIGNED)[:3,:] 62 | 63 | # vector from tool to goal, in world frame 64 | o_TG = oMtool.translation-oMgoal.translation 65 | 66 | # Control law by least square 67 | vq = -pinv(o_Jtool3)@o_TG 68 | 69 | q = pin.integrate(robot.model,q, vq * DT) 70 | viz.display(q) 71 | time.sleep(1e-3) 72 | 73 | herr.append(o_TG) 74 | # %end_jupyter_snippet 75 | 76 | # %jupyter_snippet 6d_loop 77 | q = q0.copy() 78 | herr = [] 79 | for i in range(200): # Integrate over 2 second of robot life 80 | 81 | # Run the algorithms that outputs values in robot.data 82 | pin.framesForwardKinematics(robot.model,robot.data,q) 83 | pin.computeJointJacobians(robot.model,robot.data,q) 84 | 85 | # Placement from world frame o to frame f oMtool 86 | oMtool = robot.data.oMf[IDX_TOOL] 87 | 88 | # 6D error between the two frame 89 | tool_nu = pin.log(oMtool.inverse()*oMgoal).vector 90 | 91 | # Get corresponding jacobian 92 | tool_Jtool = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_TOOL) 93 | 94 | # Control law by least square 95 | vq = pinv(tool_Jtool)@tool_nu 96 | 97 | q = pin.integrate(robot.model,q, vq * DT) 98 | viz.display(q) 99 | time.sleep(1e-3) 100 | 101 | herr.append(tool_nu) 102 | # %end_jupyter_snippet 103 | 104 | # %jupyter_snippet plot 105 | plt.subplot(211) 106 | plt.plot([ e[:3] for e in herr]) 107 | plt.xlabel('control cycle (iter)') 108 | plt.ylabel('error (m)') 109 | plt.subplot(212) 110 | plt.plot([ e[3:] for e in herr]) 111 | plt.xlabel('control cycle (iter)') 112 | plt.ylabel('error (rad)'); 113 | # %end_jupyter_snippet 114 | -------------------------------------------------------------------------------- /tp3/control_head.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Inverse kinematics (close loop / iterative) for a mobile manipulator. 3 | This second example is built upon the more simple tp3/inverse_kinematics.py. 4 | It adds a frame corresponding to the robot gaze, and provides a multi-objective control loop 5 | where the robot servo both its gripper and its gaze in two hierachized tasks. 6 | ''' 7 | 8 | import pinocchio as pin 9 | import numpy as np 10 | import time 11 | from numpy.linalg import pinv,inv,norm,svd,eig 12 | from tp3.tiago_loader import loadTiago 13 | import matplotlib.pylab as plt; plt.ion() 14 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 15 | 16 | # %jupyter_snippet robot 17 | robot = loadTiago(addGazeFrame=True) 18 | viz = MeshcatVisualizer(robot,url='classical') 19 | # %end_jupyter_snippet 20 | 21 | NQ = robot.model.nq 22 | NV = robot.model.nv 23 | IDX_TOOL = robot.model.getFrameId('frametool') 24 | IDX_BASIS = robot.model.getFrameId('framebasis') 25 | # %jupyter_snippet gaze 26 | IDX_GAZE = robot.model.getFrameId('framegaze') 27 | 28 | # Add a small ball as a visual target to be reached by the robot 29 | ball = np.array([ 1.2,0.5,1.1 ]) 30 | viz.addSphere('ball', .05, [ .8,.1,.5, .8] ) 31 | viz.applyConfiguration('ball', list(ball)+[0,0,0,1]) 32 | # %end_jupyter_snippet 33 | 34 | # Goal placement, and integration in the viewer of the goal. 35 | oMgoal = pin.SE3(pin.Quaternion(-0.5, 0.58, -0.39, 0.52).normalized().matrix(), 36 | np.array([1.2, .4, .7])) 37 | viz.addBox('goal', [.1,.1,.1], [ .1,.1,.5, .6] ) 38 | viz.applyConfiguration('goal',oMgoal) 39 | 40 | # Integration step. 41 | DT = 1e-2 42 | 43 | # Robot initial configuration. 44 | q0 = np.array([ 0. , 0. , 0. , 1. , 0.18, 1.37, -0.24, -0.98, 0.98, 45 | 0. , 0. , 0. , 0. , -0.13, 0. , 0. , 0. , 0. ]) 46 | 47 | # %jupyter_snippet gaze_loop 48 | q = q0.copy() 49 | herr = [] # Log the value of the error between gaze and ball. 50 | # Loop on an inverse kinematics for 200 iterations. 51 | for i in range(200): # Integrate over 2 second of robot life 52 | 53 | # Run the algorithms that outputs values in robot.data 54 | pin.framesForwardKinematics(robot.model,robot.data,q) 55 | pin.computeJointJacobians(robot.model,robot.data,q) 56 | 57 | # Placement from world frame o to frame f oMgaze 58 | oMgaze = robot.data.oMf[IDX_GAZE] 59 | 60 | # 6D jacobian in local frame 61 | o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:] 62 | 63 | # vector from gaze to ball, in world frame 64 | o_GazeBall = oMgaze.translation-ball 65 | 66 | vq = -pinv(o_Jgaze3) @ o_GazeBall 67 | 68 | q = pin.integrate(robot.model,q, vq * DT) 69 | viz.display(q) 70 | time.sleep(1e-3) 71 | 72 | herr.append(o_GazeBall) 73 | # %end_jupyter_snippet 74 | 75 | # %jupyter_snippet multi 76 | q = q0.copy() 77 | herr = [] # Log the value of the error between tool and goal. 78 | herr2 = [] # Log the value of the error between gaze and ball. 79 | # Loop on an inverse kinematics for 200 iterations. 80 | for i in range(200): # Integrate over 2 second of robot life 81 | 82 | pin.framesForwardKinematics(robot.model,robot.data,q) 83 | pin.computeJointJacobians(robot.model,robot.data,q) 84 | 85 | # Gaze task 86 | oMgaze = robot.data.oMf[IDX_GAZE] 87 | o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:] 88 | o_GazeBall = oMgaze.translation-ball 89 | 90 | # Tool task 91 | oMtool = robot.data.oMf[IDX_TOOL] 92 | o_Jtool3 = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL_WORLD_ALIGNED)[:3,:] 93 | o_TG = oMtool.translation-oMgoal.translation 94 | 95 | vq = -pinv(o_Jtool3) @ o_TG 96 | Ptool = np.eye(robot.nv)-pinv(o_Jtool3) @ o_Jtool3 97 | vq += pinv(o_Jgaze3 @ Ptool) @ (-o_GazeBall - o_Jgaze3 @ vq) 98 | 99 | q = pin.integrate(robot.model,q, vq * DT) 100 | viz.display(q) 101 | time.sleep(1e-3) 102 | 103 | herr.append(o_TG) 104 | herr2.append(o_GazeBall) 105 | # %end_jupyter_snippet 106 | -------------------------------------------------------------------------------- /tp7/deeptable.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Example of Q-table learning with a simple discretized 1-pendulum environment using a linear Q network. 3 | ''' 4 | 5 | import numpy as np 6 | import random 7 | import tensorflow as tf 8 | import tensorflow.compat.v1 as tf1 9 | import matplotlib.pyplot as plt 10 | from tp7.env_pendulum import EnvPendulumDiscrete; Env = lambda : EnvPendulumDiscrete(1,viewer='meshcat') 11 | import signal 12 | import time 13 | tf1.disable_eager_execution() 14 | 15 | 16 | ### --- Random seed 17 | RANDOM_SEED = int((time.time()%10)*1000) 18 | print("Seed = %d" % RANDOM_SEED) 19 | np.random.seed(RANDOM_SEED) 20 | 21 | ### --- Hyper paramaters 22 | NEPISODES = 2000 # Number of training episodes 23 | NSTEPS = 50 # Max episode length 24 | LEARNING_RATE = 0.1 # Step length in optimizer 25 | DECAY_RATE = 0.99 # Discount factor 26 | 27 | ### --- Environment 28 | env = Env() 29 | NX = env.nx 30 | NU = env.nu 31 | 32 | ### --- Q-value networks 33 | class QValueNetwork: 34 | def __init__(self): 35 | x = tf1.placeholder(shape=[1,NX],dtype=tf.float32) 36 | W = tf1.Variable(tf1.random_uniform([NX,NU],0,0.01,seed=100)) 37 | qvalue = tf1.matmul(x,W) 38 | u = tf1.argmax(qvalue,1) 39 | 40 | qref = tf1.placeholder(shape=[1,NU],dtype=tf.float32) 41 | loss = tf1.reduce_sum(tf.square(qref - qvalue)) 42 | optim = tf1.train.GradientDescentOptimizer(LEARNING_RATE).minimize(loss) 43 | 44 | self.x = x # Network input 45 | self.qvalue = qvalue # Q-value as a function of x 46 | self.u = u # Policy as a function of x 47 | self.qref = qref # Reference Q-value at next step (to be set to l+Q o f) 48 | self.optim = optim # Optimizer 49 | 50 | ### --- Tensor flow initialization 51 | #tf.reset_default_graph() 52 | qvalue = QValueNetwork() 53 | sess = tf1.InteractiveSession() 54 | tf1.global_variables_initializer().run() 55 | 56 | def onehot(ix,n=NX): 57 | '''Return a vector which is 0 everywhere except index set to 1.''' 58 | return np.array([[ (i==ix) for i in range(n) ],],np.float) 59 | 60 | def disturb(u,i): 61 | u += int(np.random.randn()*10/(i/50+10)) 62 | return np.clip(u,0,NU-1) 63 | 64 | def rendertrial(maxiter=100): 65 | x = env.reset() 66 | for i in range(maxiter): 67 | u = sess.run(qvalue.u,feed_dict={ qvalue.x:onehot(x) }) 68 | x,r = env.step(u) 69 | env.render() 70 | if r==1: print('Reward!'); break 71 | signal.signal(signal.SIGTSTP, lambda x,y:rendertrial()) # Roll-out when CTRL-Z is pressed 72 | 73 | ### --- History of search 74 | h_rwd = [] # Learning history (for plot). 75 | 76 | ### --- Training 77 | for episode in range(1,NEPISODES): 78 | x = env.reset() 79 | rsum = 0.0 80 | 81 | for step in range(NSTEPS-1): 82 | u = sess.run(qvalue.u,feed_dict={ qvalue.x: onehot(x) })[0] # Greedy policy ... 83 | u = disturb(u,episode) # ... with noise 84 | x2,reward = env.step(u) 85 | 86 | # Compute reference Q-value at state x respecting HJB 87 | Q2 = sess.run(qvalue.qvalue,feed_dict={ qvalue.x: onehot(x2) }) 88 | Qref = sess.run(qvalue.qvalue,feed_dict={ qvalue.x: onehot(x ) }) 89 | Qref[0,u] = reward + DECAY_RATE*np.max(Q2) 90 | 91 | # Update Q-table to better fit HJB 92 | sess.run(qvalue.optim,feed_dict={ qvalue.x : onehot(x), 93 | qvalue.qref : Qref }) 94 | 95 | rsum += reward 96 | x = x2 97 | if reward == 1: break 98 | 99 | h_rwd.append(rsum) 100 | if not episode%20: print('Episode #%d done with %d sucess' % (episode,sum(h_rwd[-20:]))) 101 | 102 | print("Total rate of success: %.3f" % (sum(h_rwd)/NEPISODES)) 103 | rendertrial() 104 | plt.plot( np.cumsum(h_rwd)/range(1,NEPISODES) ) 105 | plt.show() 106 | 107 | -------------------------------------------------------------------------------- /tp2/simple_pick_and_place.py: -------------------------------------------------------------------------------- 1 | import math 2 | import time 3 | import unittest 4 | 5 | import numpy as np 6 | import pinocchio as pin 7 | import example_robot_data as robex 8 | 9 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors 10 | 11 | # %jupyter_snippet 1 12 | robot = robex.load('ur5') 13 | # %end_jupyter_snippet 14 | NQ = robot.model.nq 15 | NV = robot.model.nv 16 | 17 | # Open the viewer 18 | viz = MeshcatVisualizer(robot) 19 | 20 | # %jupyter_snippet 2 21 | # Add a red box in the viewer 22 | ballID = "world/ball" 23 | viz.addSphere(ballID, 0.1, colors.red) 24 | 25 | # Place the ball at the position ( 0.5, 0.1, 0.2 ) 26 | # The viewer expect position and rotation, apppend the identity quaternion 27 | q_ball = [0.5, 0.1, 0.2, 1, 0, 0, 0] 28 | viz.applyConfiguration(ballID, q_ball) 29 | # %end_jupyter_snippet 30 | 31 | # 32 | # PICK ############################################################# 33 | # 34 | 35 | # Configuration for picking the box 36 | # %jupyter_snippet 3 37 | q0 = np.zeros(NQ) # set the correct values here 38 | q0[0] = -0.375 39 | q0[1] = -1.2 40 | q0[2] = 1.71 41 | q0[3] = -q0[1] - q0[2] 42 | q0[4] = q0[0] 43 | q0[5] = 0. 44 | 45 | viz.display(q0) 46 | q = q0.copy() 47 | # %end_jupyter_snippet 48 | print("The robot is display with end effector on the red ball.") 49 | 50 | # 51 | # MOVE 3D ############################################################# 52 | # 53 | 54 | print("Let's start the movement ...") 55 | 56 | # %jupyter_snippet 4 57 | # Random velocity of the robot driving the movement 58 | vq = np.array([2., 0, 0, 4., 0, 0]) 59 | 60 | idx = robot.index('wrist_3_joint') 61 | o_eff = robot.placement(q, idx).translation # Position of end-eff wrt world at current configuration 62 | o_ball = q_ball[:3] # Position of ball wrt world 63 | eff_ball = o_ball - o_eff # Position of ball wrt eff 64 | 65 | for i in range(50): 66 | # Chose new configuration of the robot 67 | q += vq / 40 68 | q[2] = 1.71 + math.sin(i * 0.05) / 2 69 | 70 | # Gets the new position of the ball 71 | o_ball = robot.placement(q, idx) * eff_ball 72 | 73 | # Display new configuration for robot and ball 74 | viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0]) 75 | viz.display(q) 76 | time.sleep(1e-2) 77 | # %end_jupyter_snippet 78 | 79 | # 80 | # MOVE 6D ############################################################# 81 | # 82 | 83 | q = q0.copy() 84 | viz.display(q0) 85 | 86 | # %jupyter_snippet 5 87 | # Add a red box in the viewer 88 | boxID = "world/box" 89 | #viz.delete(ballID) 90 | viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta) 91 | 92 | # Place the box at the position (0.5, 0.1, 0.2) 93 | q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0] 94 | viz.applyConfiguration(boxID, q_box) 95 | viz.applyConfiguration(ballID, [2,2,2,1,0,0,0]) 96 | # %end_jupyter_snippet 97 | 98 | # %jupyter_snippet 6 99 | q0 = np.zeros(NQ) 100 | q0[0] = -0.375 101 | q0[1] = -1.2 102 | q0[2] = 1.71 103 | q0[3] = -q0[1] - q0[2] 104 | q0[4] = q0[0] 105 | 106 | viz.display(q0) 107 | q = q0.copy() 108 | # %end_jupyter_snippet 109 | 110 | print("Now moving with a 6D object ... ") 111 | 112 | # %jupyter_snippet 7 113 | # Random velocity of the robot driving the movement 114 | vq = np.array([2., 0, 0, 4., 0, 0]) 115 | 116 | idx = robot.index('wrist_3_joint') 117 | oMeff = robot.placement(q, idx) # Placement of end-eff wrt world at current configuration 118 | oMbox = pin.XYZQUATToSE3(q_box) # Placement of box wrt world 119 | effMbox = oMeff.inverse() * oMbox # Placement of box wrt eff 120 | 121 | for i in range(100): 122 | # Chose new configuration of the robot 123 | q += vq / 40 124 | q[2] = 1.71 + math.sin(i * 0.05) / 2 125 | 126 | # Gets the new position of the box 127 | oMbox = robot.placement(q, idx) * effMbox 128 | 129 | # Display new configuration for robot and box 130 | viz.applyConfiguration(boxID, oMbox) 131 | viz.display(q) 132 | time.sleep(1e-2) 133 | # %end_jupyter_snippet 134 | 135 | ########################################################################## 136 | ### This last part is to automatically validate the versions of this example. 137 | class SimplePickAndPlaceTest(unittest.TestCase): 138 | def test_oMbox_translation(self): 139 | self.assertTrue((np.abs(oMbox.translation - np.array([ 0.22085156, -0.6436716 , 0.5632217 ])) < 1e-5).all()) 140 | -------------------------------------------------------------------------------- /tp5/arm_example.py: -------------------------------------------------------------------------------- 1 | ''' 2 | # In this example test, we will solve the reaching-goal task with the Talos arm. 3 | # For that, we use the forward dynamics (with its analytical derivatives) 4 | # developed inside crocoddyl; it describes inside DifferentialActionModelFullyActuated class. 5 | # Finally, we use an Euler sympletic integration scheme. 6 | ''' 7 | 8 | import sys 9 | WITHDISPLAY = 'display' in sys.argv 10 | WITHPLOT = 'plot' in sys.argv 11 | 12 | import crocoddyl; crocoddyl.switchToNumpyMatrix() 13 | import pinocchio 14 | import numpy as np 15 | import example_robot_data 16 | 17 | # First, let's load the Pinocchio model for the Talos arm. 18 | robot = example_robot_data.load('talos_arm') 19 | 20 | # Set robot model 21 | robot_model = robot.model 22 | robot_model.armature =np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.])*5 23 | robot_model.q0 = np.array([3.5,2,2,0,0,0,0]) 24 | robot_model.x0 = np.concatenate([robot_model.q0, pinocchio.utils.zero(robot_model.nv)]) 25 | robot_model.gravity *= 0 26 | 27 | # Configure task 28 | FRAME_TIP = robot_model.getFrameId("gripper_left_fingertip_3_link") 29 | goal = np.array([.2,0.5,.5]) 30 | 31 | # Configure viewer 32 | from tp3.meshcat_viewer_wrapper import MeshcatVisualizer 33 | viz = MeshcatVisualizer(robot,url='tcp://127.0.0.1:6004')#classical') 34 | viz.display(robot_model.q0) 35 | viz.addBox('world/box',[.1,.1,.1], [1.,0,0,1]) 36 | viz.addBox('world/goal',[.1,.1,.1],[0,1,0,1]) 37 | viz.applyConfiguration('world/goal',[0.2,0.5,.5,0,0,0,1]) 38 | 39 | # Create a cost model per the running and terminal action model. 40 | state = crocoddyl.StateMultibody(robot_model) 41 | runningCostModel = crocoddyl.CostModelSum(state) 42 | terminalCostModel = crocoddyl.CostModelSum(state) 43 | 44 | # Note that we need to include a cost model (i.e. set of cost functions) in 45 | # order to fully define the action model for our optimal control problem. 46 | # For this particular example, we formulate three running-cost functions: 47 | # goal-tracking cost, state and control regularization; and one terminal-cost: 48 | # goal cost. First, let's create the common cost functions. 49 | Mref = crocoddyl.FramePlacement(FRAME_TIP,pinocchio.SE3(np.eye(3), goal)) 50 | #goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref) 51 | pref = crocoddyl.FrameTranslation(FRAME_TIP,goal) 52 | goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, pref) 53 | weights=crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1,1,1,1,1, 1,1,1,1,2,2,2.])) 54 | xRegCost = crocoddyl.CostModelState(state,weights,robot_model.x0) 55 | uRegCost = crocoddyl.CostModelControl(state) 56 | weightsT=crocoddyl.ActivationModelWeightedQuad(np.array([.01,.01,.01,.01,.01,.01,.01, 1,1,1,1,2,2,2.])) 57 | xRegCostT = crocoddyl.CostModelState(state,weights,robot_model.x0) 58 | 59 | # Then let's added the running and terminal cost functions 60 | runningCostModel.addCost("gripperPose", goalTrackingCost, .001) 61 | runningCostModel.addCost("xReg", xRegCost, 1e-3) 62 | runningCostModel.addCost("uReg", uRegCost, 1e-6) 63 | terminalCostModel.addCost("gripperPose", goalTrackingCost, 10) 64 | terminalCostModel.addCost("xReg", xRegCostT, .01) 65 | 66 | # Next, we need to create an action model for running and terminal knots. The 67 | # forward dynamics (computed using ABA) are implemented 68 | # inside DifferentialActionModelFullyActuated. 69 | actuationModel = crocoddyl.ActuationModelFull(state) 70 | dt = 1e-2 71 | runningModel = crocoddyl.IntegratedActionModelEuler( 72 | crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, runningCostModel), dt) 73 | runningModel.differential.armature = robot_model.armature 74 | terminalModel = crocoddyl.IntegratedActionModelEuler( 75 | crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.) 76 | terminalModel.differential.armature = robot_model.armature 77 | 78 | # For this optimal control problem, we define 250 knots (or running action 79 | # models) plus a terminal knot 80 | T = 100 81 | problem = crocoddyl.ShootingProblem(robot_model.x0, [runningModel] * T, terminalModel) 82 | 83 | # Creating the DDP solver for this OC problem, defining a logger 84 | ddp = crocoddyl.SolverDDP(problem) 85 | ddp.setCallbacks([ 86 | crocoddyl.CallbackLogger(), 87 | crocoddyl.CallbackVerbose(), 88 | ]) 89 | 90 | # Solving it with the DDP algorithm 91 | ddp.solve([],[],1000) # xs_init,us_init,maxiter 92 | 93 | # Plotting the solution and the DDP convergence 94 | if WITHPLOT: 95 | log = ddp.getCallbacks()[0] 96 | crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False) 97 | crocoddyl.plotConvergence(log.costs, log.u_regs, log.x_regs, log.grads, log.stops, log.steps, figIndex=2) 98 | 99 | # Visualizing the solution in gepetto-viewer 100 | if WITHDISPLAY: 101 | import tp7.croco_utils as crocutils 102 | crocutils.displayTrajectory(viz,ddp.xs,ddp.problem.runningModels[0].dt,12) 103 | -------------------------------------------------------------------------------- /tp7/qlearn.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Train a Q-value following a classical Q-learning algorithm (enforcing the 3 | satisfaction of HJB method), using a noisy greedy exploration strategy. 4 | 5 | The result of a training for a continuous pendulum (after 200 iterations) 6 | are stored in qvalue.h5. 7 | 8 | Reference: 9 | Mnih, Volodymyr, et al. "Human-level control through deep reinforcement learning." 10 | Nature 518.7540 (2015): 529. 11 | ''' 12 | 13 | from tp7.env_pendulum import EnvPendulumHybrid; Env = lambda : EnvPendulumHybrid(1,viewer='meshcat') 14 | from tp7.qnetwork import QNetwork 15 | from collections import deque 16 | import time 17 | import signal 18 | import matplotlib.pyplot as plt 19 | import random 20 | import numpy as np 21 | import tensorflow as tf 22 | 23 | ### --- Random seed 24 | RANDOM_SEED = int((time.time()%10)*1000) 25 | print("Seed = %d" % RANDOM_SEED) 26 | np .random.seed (RANDOM_SEED) 27 | random.seed (RANDOM_SEED) 28 | 29 | ### --- Environment 30 | env = Env() 31 | 32 | ### --- Hyper paramaters 33 | NEPISODES = 1000 # Max training steps 34 | NSTEPS = 60 # Max episode length 35 | QVALUE_LEARNING_RATE = 0.001 # Base learning rate for the Q-value Network 36 | DECAY_RATE = 0.99 # Discount factor 37 | UPDATE_RATE = 0.01 # Homotopy rate to update the networks 38 | REPLAY_SIZE = 10000 # Size of replay buffer 39 | BATCH_SIZE = 64 # Number of points to be fed in stochastic gradient 40 | NH1 = NH2 = 32 # Hidden layer size 41 | 42 | ### --- Replay memory 43 | class ReplayItem: 44 | def __init__(self,x,u,r,d,x2): 45 | self.x = x 46 | self.u = u 47 | self.reward = r 48 | self.done = d 49 | self.x2 = x2 50 | replayDeque = deque() 51 | 52 | ### --- Tensor flow initialization 53 | qvalue = QNetwork(nx=env.nx,nu=env.nu,learning_rate=QVALUE_LEARNING_RATE) 54 | qvalueTarget = QNetwork(name='target',nx=env.nx,nu=env.nu) 55 | # Uncomment to load networks 56 | #qvalue.load() 57 | #qvalueTarget.load() 58 | 59 | def rendertrial(maxiter=NSTEPS,verbose=True): 60 | x = env.reset() 61 | traj = [x.copy()] 62 | rsum = 0. 63 | for i in range(maxiter): 64 | u = qvalue.policy(x)[0] 65 | x, reward = env.step(u) 66 | env.render() 67 | time.sleep(1e-2) 68 | rsum += reward 69 | traj.append(x.copy()) 70 | if verbose: print('Lasted ',i,' timestep -- total reward:',rsum) 71 | return np.array(traj) 72 | signal.signal(signal.SIGTSTP, lambda x,y:rendertrial()) # Roll-out when CTRL-Z is pressed 73 | 74 | ### History of search 75 | h_rwd = [] 76 | 77 | ### --- Training 78 | for episode in range(1,NEPISODES): 79 | x = env.reset() 80 | rsum = 0.0 81 | 82 | for step in range(NSTEPS): 83 | u = qvalue.policy(x, # Greedy policy ... 84 | noise=1. / (1. + episode + step)) # ... with noise 85 | x2,r = env.step(u) 86 | done = False # Some environment may return information when task completed 87 | 88 | replayDeque.append(ReplayItem(x,u,r,done,x2)) # Feed replay memory ... 89 | if len(replayDeque)>REPLAY_SIZE: replayDeque.popleft() # ... with FIFO forgetting. 90 | 91 | rsum += r 92 | x = x2 93 | if done: break 94 | 95 | # Start optimizing networks when memory size > batch size. 96 | if len(replayDeque) > BATCH_SIZE: 97 | batch = random.sample(replayDeque,BATCH_SIZE) # Random batch from replay memory. 98 | x_batch = np.vstack([ b.x for b in batch ]) 99 | u_batch = np.vstack([ b.u for b in batch ]) 100 | r_batch = np.array([ [b.reward] for b in batch ]) 101 | d_batch = np.array([ [b.done] for b in batch ]) 102 | x2_batch = np.vstack([ b.x2 for b in batch ]) 103 | 104 | # Compute Q(x,u) from target network 105 | v_batch = qvalueTarget.value(x2_batch) 106 | qref_batch = r_batch + (d_batch==False)*(DECAY_RATE*v_batch) 107 | 108 | # Update qvalue to solve HJB constraint: q = r + q' 109 | qvalue.trainer.train_on_batch([x_batch,u_batch],qref_batch) 110 | 111 | # Update target networks by homotopy. 112 | qvalueTarget.targetAssign(qvalue,UPDATE_RATE) 113 | 114 | # \\\END_FOR step in range(NSTEPS) 115 | 116 | # Display and logging (not mandatory). 117 | print('Ep#{:3d}: lasted {:d} steps, reward={:3.0f}' .format(episode, step,rsum)) 118 | h_rwd.append(rsum) 119 | if not (episode+1) % 200: rendertrial(30) 120 | 121 | # \\\END_FOR episode in range(NEPISODES) 122 | 123 | print("Average reward during trials: %.3f" % (sum(h_rwd)/NEPISODES)) 124 | rendertrial() 125 | plt.plot( np.cumsum(h_rwd)/range(1,NEPISODES) ) 126 | plt.show() 127 | 128 | # Uncomment to save networks 129 | #qvalue.save() 130 | -------------------------------------------------------------------------------- /utils/load_ur5_with_obstacles.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Load a UR5 robot model, display it in the viewer. Also create an obstacle 3 | field made of several capsules, display them in the viewer and create the 4 | collision detection to handle it. 5 | ''' 6 | 7 | import pinocchio as pin 8 | import example_robot_data as robex 9 | import numpy as np 10 | import itertools 11 | 12 | 13 | def XYZRPYtoSE3(xyzrpy): 14 | rotate = pin.utils.rotate 15 | R = rotate('x',xyzrpy[3]) @ rotate('y',xyzrpy[4]) @ rotate('z',xyzrpy[5]) 16 | p = np.array(xyzrpy[:3]) 17 | return pin.SE3(R,p) 18 | 19 | def load_ur5_with_obstacles(robotname='ur5',reduced=False): 20 | 21 | ### Robot 22 | # Load the robot 23 | robot = robex.load(robotname) 24 | 25 | ### If reduced, then only keep should-tilt and elbow joint, hence creating a simple R2 robot. 26 | if reduced: 27 | unlocks = [1,2] 28 | robot.model,[robot.visual_model,robot.collision_model]\ 29 | = pin.buildReducedModel(robot.model,[robot.visual_model,robot.collision_model], 30 | [ i+1 for i in range(robot.nq) if i not in unlocks ],robot.q0) 31 | robot.data = robot.model.createData() 32 | robot.collision_data = robot.collision_model.createData() 33 | robot.visual_data = robot.visual_model.createData() 34 | robot.q0 = robot.q0[unlocks].copy() 35 | 36 | ### Obstacle map 37 | # Capsule obstacles will be placed at these XYZ-RPY parameters 38 | oMobs = [ [ 0.40, 0., 0.30, np.pi/2,0,0], 39 | [-0.08, -0., 0.69, np.pi/2,0,0], 40 | [ 0.23, -0., 0.04, np.pi/2, 0 ,0 ], 41 | [-0.32, 0., -0.08, np.pi/2, 0, 0]] 42 | 43 | # Load visual objects and add them in collision/visual models 44 | color = [ 1.0, 0.2, 0.2, 1.0 ] # color of the capsules 45 | rad,length = .1,0.4 # radius and length of capsules 46 | for i,xyzrpy in enumerate(oMobs): 47 | obs = pin.GeometryObject.CreateCapsule(rad,length) # Pinocchio obstacle object 48 | obs.meshColor = np.array([ 1.0, 0.2, 0.2, 1.0 ]) # Don't forget me, otherwise I am transparent ... 49 | obs.name = "obs%d"%i # Set object name 50 | obs.parentJoint = 0 # Set object parent = 0 = universe 51 | obs.placement = XYZRPYtoSE3(xyzrpy) # Set object placement wrt parent 52 | robot.collision_model.addGeometryObject(obs) # Add object to collision model 53 | robot.visual_model .addGeometryObject(obs) # Add object to visual model 54 | 55 | ### Collision pairs 56 | nobs = len(oMobs) 57 | nbodies = robot.collision_model.ngeoms-nobs 58 | robotBodies = range(nbodies) 59 | envBodies = range(nbodies,nbodies+nobs) 60 | robot.collision_model.removeAllCollisionPairs() 61 | for a,b in itertools.product(robotBodies,envBodies): 62 | robot.collision_model.addCollisionPair(pin.CollisionPair(a,b)) 63 | 64 | ### Geom data 65 | # Collision/visual models have been modified => re-generate corresponding data. 66 | robot.collision_data = pin.GeometryData(robot.collision_model) 67 | robot.visual_data = pin.GeometryData(robot.visual_model ) 68 | 69 | return robot 70 | 71 | 72 | class Target: 73 | ''' 74 | Simple class target that stores and display the position of a target. 75 | ''' 76 | def __init__(self,viz=None,color = [ .0, 1.0, 0.2, 1.0 ], radius = 0.05, position=None): 77 | self.position = position if position is not None else np.array([ 0.0, 0.0 ]) 78 | self.initVisual(viz,color,radius) 79 | self.display() 80 | 81 | def initVisual(self,viz,color,radius): 82 | self.viz = viz 83 | if viz is None: return 84 | self.name = "world/pinocchio/target" 85 | 86 | if isinstance(viz,pin.visualize.MeshcatVisualizer): 87 | import meshcat 88 | obj = meshcat.geometry.Sphere(radius) 89 | material = meshcat.geometry.MeshPhongMaterial() 90 | material.color = int(color[0] * 255) * 256**2 + int(color[1] * 255) * 256 + int(color[2] * 255) 91 | if float(color[3]) != 1.0: 92 | material.transparent = True 93 | material.opacity = float(color[3]) 94 | self.viz.viewer[self.name].set_object(obj, material) 95 | 96 | elif isinstance(viz,pin.visualize.GepettoVisualizer): 97 | self.viz.viewer.gui.addCapsule( self.name, radius,0., color) 98 | 99 | def display(self): 100 | if self.viz is None or self.position is None: return 101 | 102 | if isinstance(self.viz,pin.visualize.MeshcatVisualizer): 103 | T = np.eye(4) 104 | T[[0,2],3] = self.position 105 | self.viz.viewer[self.name].set_transform(T) 106 | elif isinstance(self.viz,pin.visualize.GepettoVisualizer): 107 | self.viz.viewer.gui.applyConfiguration( self.name, 108 | [ self.position[0], 0, self.position[1], 109 | 1.,0.,0.0,0. ]) 110 | self.viz.viewer.gui.refresh() 111 | -------------------------------------------------------------------------------- /tp3/tiago_loader.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Tiago loader accounting for the first planar joint giving robot mobility. 3 | ''' 4 | 5 | import numpy as np 6 | import pinocchio as pin 7 | import example_robot_data as robex 8 | import hppfcl 9 | from os.path import dirname, exists, join 10 | 11 | 12 | class TiagoLoader(object): 13 | #path = '' 14 | #urdf_filename = '' 15 | srdf_filename = '' 16 | urdf_subpath = 'robots' 17 | srdf_subpath = 'srdf' 18 | ref_posture = 'half_sitting' 19 | has_rotor_parameters = False 20 | free_flyer = True 21 | verbose = False 22 | path = "tiago_description" 23 | urdf_filename = "tiago_no_hand.urdf" 24 | 25 | def __init__(self): 26 | urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename) 27 | self.model_path = robex.getModelPath(urdf_path, self.verbose) 28 | self.urdf_path = join(self.model_path, urdf_path) 29 | self.robot = pin.RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')], 30 | pin.JointModelPlanar() if self.free_flyer else None) 31 | 32 | if self.srdf_filename: 33 | self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename) 34 | self.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose, self.has_rotor_parameters, 35 | self.ref_posture) 36 | else: 37 | self.srdf_path = None 38 | self.q0 = None 39 | 40 | if self.free_flyer: 41 | self.addFreeFlyerJointLimits() 42 | 43 | def addFreeFlyerJointLimits(self): 44 | ub = self.robot.model.upperPositionLimit 45 | ub[:self.robot.model.joints[1].nq] = 1 46 | self.robot.model.upperPositionLimit = ub 47 | lb = self.robot.model.lowerPositionLimit 48 | lb[:self.robot.model.joints[1].nq] = -1 49 | self.robot.model.lowerPositionLimit = lb 50 | 51 | 52 | def loadTiago(addGazeFrame=False): 53 | ''' 54 | Load a tiago model, without the hand, and with the two following modifications wrt example_robot_data. 55 | - first, the first joint is a planar (x,y,cos,sin) joint, while it is a fixed robot in example robot data. 56 | - second, two visual models of a frame have been added to two new op-frame, "tool0" on the robot hand, and "basis0" in 57 | front of the basis. 58 | ''' 59 | 60 | 61 | robot = TiagoLoader().robot 62 | geom = robot.visual_model 63 | 64 | X = pin.utils.rotate('y', np.pi/2) 65 | Y = pin.utils.rotate('x',-np.pi/2) 66 | Z = np.eye(3) 67 | 68 | L = .3 69 | cyl=hppfcl.Cylinder(L/30,L) 70 | med = np.array([0,0,L/2]) 71 | 72 | # --------------------------------------------------------------------------- 73 | # Add a frame visualisation in the effector. 74 | 75 | FIDX = robot.model.getFrameId('wrist_ft_tool_link') 76 | JIDX = robot.model.frames[FIDX].parent 77 | 78 | eff = np.array([0,0,.08]) 79 | FIDX = robot.model.addFrame(pin.Frame('frametool',JIDX,FIDX,pin.SE3(Z,eff),pin.FrameType.OP_FRAME)) 80 | 81 | geom.addGeometryObject(pin.GeometryObject('axis_x',FIDX,JIDX,cyl,pin.SE3(X,X@med+eff))) 82 | geom.geometryObjects[-1].meshColor = np.array([1,0,0,1.]) 83 | 84 | geom.addGeometryObject(pin.GeometryObject('axis_y',FIDX,JIDX,cyl,pin.SE3(Y,Y@med+eff))) 85 | geom.geometryObjects[-1].meshColor = np.array([0,1,0,1.]) 86 | 87 | geom.addGeometryObject(pin.GeometryObject('axis_z',FIDX,JIDX,cyl,pin.SE3(Z,Z@med+eff))) 88 | geom.geometryObjects[-1].meshColor = np.array([0,0,1,1.]) 89 | 90 | # --------------------------------------------------------------------------- 91 | # Add a frame visualisation in front of the basis. 92 | 93 | FIDX = robot.model.getFrameId('base_link') 94 | JIDX = robot.model.frames[FIDX].parent 95 | 96 | eff = np.array([.3,0,.15]) 97 | FIDX = robot.model.addFrame(pin.Frame('framebasis',JIDX,FIDX,pin.SE3(Z,eff),pin.FrameType.OP_FRAME)) 98 | 99 | geom.addGeometryObject(pin.GeometryObject('axis2_x',FIDX,JIDX,cyl,pin.SE3(X,X@med+eff))) 100 | geom.geometryObjects[-1].meshColor = np.array([1,0,0,1.]) 101 | 102 | geom.addGeometryObject(pin.GeometryObject('axi2_y',FIDX,JIDX,cyl,pin.SE3(Y,Y@med+eff))) 103 | geom.geometryObjects[-1].meshColor = np.array([0,1,0,1.]) 104 | 105 | geom.addGeometryObject(pin.GeometryObject('axis2_z',FIDX,JIDX,cyl,pin.SE3(Z,Z@med+eff))) 106 | geom.geometryObjects[-1].meshColor = np.array([0,0,1,1.]) 107 | 108 | # --------------------------------------------------------------------------- 109 | # Add a frame visualisation in front of the head. 110 | 111 | if addGazeFrame: 112 | L = .05 113 | cyl=hppfcl.Cylinder(L/30,L) 114 | med = np.array([0,0,L/2]) 115 | 116 | FIDX = robot.model.getFrameId('xtion_joint') 117 | JIDX = robot.model.frames[FIDX].parent 118 | 119 | eff = np.array([0.4,0.0,0.0]) 120 | FIDX = robot.model.addFrame(pin.Frame('framegaze',JIDX,FIDX,pin.SE3(Z,eff),pin.FrameType.OP_FRAME)) 121 | 122 | geom.addGeometryObject(pin.GeometryObject('axisgaze_x',FIDX,JIDX,cyl,pin.SE3(X,X@med+eff))) 123 | geom.geometryObjects[-1].meshColor = np.array([1,0,0,1.]) 124 | 125 | geom.addGeometryObject(pin.GeometryObject('axisgaze_y',FIDX,JIDX,cyl,pin.SE3(Y,Y@med+eff))) 126 | geom.geometryObjects[-1].meshColor = np.array([0,1,0,1.]) 127 | 128 | geom.addGeometryObject(pin.GeometryObject('axisgaze_z',FIDX,JIDX,cyl,pin.SE3(Z,Z@med+eff))) 129 | geom.geometryObjects[-1].meshColor = np.array([0,0,1,1.]) 130 | 131 | # ------------------------------------------------------------------------------- 132 | # Regenerate the data from the new models. 133 | 134 | robot.q0 = np.array([1,1,1,0]+[0]*(robot.model.nq-4)) 135 | 136 | robot.data = robot.model.createData() 137 | robot.visual_data = robot.visual_model.createData() 138 | 139 | return robot 140 | 141 | # ------------------------------------------------------------------------------------------------ 142 | # ------------------------------------------------------------------------------------------------ 143 | # ------------------------------------------------------------------------------------------------ 144 | 145 | if __name__ == "__main__": 146 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 147 | 148 | robot = loadTiago() 149 | viz = MeshcatVisualizer(robot,url='classical') 150 | 151 | viz.display(robot.q0) 152 | 153 | -------------------------------------------------------------------------------- /C_scipy_optimizers.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Optimizers in SciPy\n", 8 | "This notebook is a very brief introduction to SciPy optimizers, documenting the example appendix/scipy_optim.py." 9 | ] 10 | }, 11 | { 12 | "cell_type": "markdown", 13 | "metadata": {}, 14 | "source": [ 15 | "There are several optimizers in SciPy, in the module scipy.optimize. You can simply install them with +pip install scipy. \n", 16 | "You may find the user manual of this module in https://docs.scipy.org/doc/scipy/tutorial/optimize.html#tutorial-sqlsp.\n", 17 | "\n", 18 | "In this serie of notebooks about robotics, we mostly use BFGS, a quasi-Newton constraint-free algorithm, and SLSQP, a sequential QP solver accepting both equality and inequality constraints.\n", 19 | "\n", 20 | "We will then need the two +fmin functions from the scipy.optimize module, as well as +numpy to represent algebraic vectors." 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "import numpy as np\n", 30 | "from scipy.optimize import fmin_bfgs, fmin_slsqp\n" 31 | ] 32 | }, 33 | { 34 | "cell_type": "markdown", 35 | "metadata": {}, 36 | "source": [ 37 | "\n", 38 | "They are generally following a similar API, taking as main argument the cost function to optimize +f, the initial guess +x0, and optiminally a callback function +callback and some constraints.\n", 39 | "\n", 40 | "The cost objective should be defined as a function mapping the parameter space $x$ to a real value $f(x)$. Here is a simple polynomial example for $x \\in R^2$:" 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "metadata": {}, 47 | "outputs": [], 48 | "source": [ 49 | "def cost(x):\n", 50 | " '''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x '''\n", 51 | " x0 = x[0]\n", 52 | " x1 = x[1]\n", 53 | " return -1 * (2 * x0 * x1 + 2 * x0 - x0**2 - 2 * x1**2)\n" 54 | ] 55 | }, 56 | { 57 | "cell_type": "markdown", 58 | "metadata": {}, 59 | "source": [ 60 | "The callback takes the same signature but returns nothing: it only works by side effect, for example printing something, or displaying some informations in a viewer or on a plot, or possibly storing data in a logger. Here is for example a callback written as the functor of an object, that can be used to adjust its behavior or store some data." 61 | ] 62 | }, 63 | { 64 | "cell_type": "code", 65 | "execution_count": null, 66 | "metadata": {}, 67 | "outputs": [], 68 | "source": [ 69 | "class CallbackLogger:\n", 70 | " def __init__(self):\n", 71 | " self.nfeval = 1\n", 72 | "\n", 73 | " def __call__(self, x):\n", 74 | " print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))\n", 75 | " self.nfeval += 1\n" 76 | ] 77 | }, 78 | { 79 | "cell_type": "markdown", 80 | "metadata": {}, 81 | "source": [ 82 | "For BFGS, that's all we need, as it does not accept any additional constraints. " 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "x0 = np.array([0.0, 0.0])\n", 92 | "# Optimize cost without any constraints in BFGS, with traces.\n", 93 | "xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger())\n", 94 | "print('\\n *** Xopt in BFGS = %s \\n\\n\\n\\n' % str(xopt_bfgs))\n" 95 | ] 96 | }, 97 | { 98 | "cell_type": "markdown", 99 | "metadata": {}, 100 | "source": [ 101 | "In that case, the gradients of the cost are computed by BFGS using finite differencing (i.e. not very accurately, but the algorithmic cost is typically very bad). If you can provide some derivatives by yourself, it would greatly improve the result. Yet, as a first draft, it is generally not too bad." 102 | ] 103 | }, 104 | { 105 | "cell_type": "markdown", 106 | "metadata": {}, 107 | "source": [ 108 | "For SLSQP, you can simply do the same." 109 | ] 110 | }, 111 | { 112 | "cell_type": "code", 113 | "execution_count": null, 114 | "metadata": {}, 115 | "outputs": [], 116 | "source": [ 117 | "# Optimize cost without any constraints in CLSQ\n", 118 | "xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1)\n", 119 | "print('\\n *** Xopt in LSQ = %s \\n\\n\\n\\n' % str(xopt_lsq))\n" 120 | ] 121 | }, 122 | { 123 | "cell_type": "markdown", 124 | "metadata": {}, 125 | "source": [ 126 | "Now, SLSQP can also handle explicit constraints. Equality and inequality constraints must be given separately as function from the parameter $x$ to a vector stacking all the numerical quantities, that must be null for equalities, and positive for inequalities.\n", 127 | "\n", 128 | "We introduce here, as an example, two set of polynomial constraints." 129 | ] 130 | }, 131 | { 132 | "cell_type": "code", 133 | "execution_count": null, 134 | "metadata": {}, 135 | "outputs": [], 136 | "source": [ 137 | "def constraint_eq(x):\n", 138 | " ''' Constraint x^3 = y '''\n", 139 | " return np.array([x[0]**3 - x[1]])\n", 140 | "\n", 141 | "def constraint_ineq(x):\n", 142 | " '''Constraint x>=2, y>=2'''\n", 143 | " return np.array([x[0] - 2, x[1] - 2])\n" 144 | ] 145 | }, 146 | { 147 | "cell_type": "markdown", 148 | "metadata": {}, 149 | "source": [ 150 | "The solver then run as follows:" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": null, 156 | "metadata": {}, 157 | "outputs": [], 158 | "source": [ 159 | "# Optimize cost with equality and inequality constraints in CLSQ\n", 160 | "xopt_clsq = fmin_slsqp(cost, [-1.0, 1.0], f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, iprint=2, full_output=1)\n", 161 | "print('\\n *** Xopt in c-lsq = %s \\n\\n\\n\\n' % str(xopt_clsq))\n" 162 | ] 163 | }, 164 | { 165 | "cell_type": "markdown", 166 | "metadata": {}, 167 | "source": [ 168 | "That's all for now, folks." 169 | ] 170 | }, 171 | { 172 | "cell_type": "code", 173 | "execution_count": null, 174 | "metadata": {}, 175 | "outputs": [], 176 | "source": [] 177 | } 178 | ], 179 | "metadata": { 180 | "kernelspec": { 181 | "display_name": "Python 3 (ipykernel)", 182 | "language": "python", 183 | "name": "python3" 184 | }, 185 | "language_info": { 186 | "codemirror_mode": { 187 | "name": "ipython", 188 | "version": 3 189 | }, 190 | "file_extension": ".py", 191 | "mimetype": "text/x-python", 192 | "name": "python", 193 | "nbconvert_exporter": "python", 194 | "pygments_lexer": "ipython3", 195 | "version": "3.9.13" 196 | } 197 | }, 198 | "nbformat": 4, 199 | "nbformat_minor": 4 200 | } 201 | -------------------------------------------------------------------------------- /tp7/qnetwork.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Deep Q learning, i.e. learning the Q function Q(x,u) so that Pi(x) = u = argmax Q(x,u) 3 | is the optimal policy. The control u is discretized as 0..NU-1 4 | 5 | This program instantiates an environment env and a Q network qvalue. 6 | The main signals are qvalue.x (state input), qvalue.qvalues (value for any u in 0..NU-1), 7 | qvalue.policy (i.e. argmax(qvalue.qvalues)) and qvalue.qvalue (i.e. max(qvalue.qvalue)). 8 | 9 | Reference: 10 | Mnih, Volodymyr, et al. "Human-level control through deep reinforcement learning." 11 | Nature 518.7540 (2015): 529. 12 | ''' 13 | 14 | import tensorflow as tf 15 | import numpy as np 16 | from tensorflow.keras import layers 17 | import tensorflow.keras as keras 18 | import tensorflow.keras.backend as K 19 | 20 | def batch_gather(reference, indices): 21 | """ 22 | From https://github.com/keras-team/keras/pull/6377 (not merged). 23 | 24 | Batchwise gathering of row indices. 25 | The numpy equivalent is `reference[np.arange(batch_size), indices]`, where 26 | `batch_size` is the first dimension of the reference tensor. 27 | # Arguments 28 | reference: A tensor with ndim >= 2 of shape. 29 | (batch_size, dim1, dim2, ..., dimN) 30 | indices: A 1d integer tensor of shape (batch_size) satisfying 31 | 0 <= i < dim2 for each element i. 32 | # Returns 33 | The selected tensor with shape (batch_size, dim2, ..., dimN). 34 | # Examples 35 | 1. If reference is `[[3, 5, 7], [11, 13, 17]]` and indices is `[2, 1]` 36 | then the result is `[7, 13]`. 37 | """ 38 | batch_size = keras.backend.shape(reference)[0] 39 | indices = tf.concat([tf.reshape(tf.range(batch_size),[batch_size,1]), 40 | indices],1) 41 | return tf.gather_nd(reference,indices=indices) 42 | 43 | 44 | class QNetwork: 45 | ''' 46 | Build a keras model computing: 47 | - qvalues(x) = [ Q(x,u_1) ... Q(x,u_NU) ] 48 | - value(x) = max_u qvalues(x) 49 | - qvalue(x,u) = Q(x,u) 50 | ''' 51 | def __init__(self,nx,nu,name='',nhiden=32,learning_rate=None): 52 | ''' 53 | The network has the following structure: 54 | 55 | x => [ DENSE1 ] => [ DENSE2 ] => [ QVALUES ] ==========MAX=> VALUE(x) 56 | \=>[ ] 57 | u ============================================>[ NGATHER ] => QVALUE(x,u) 58 | 59 | where: 60 | - qvalues(x) = [ qvalue(x,u=0) ... qvalue(x,u=NU-1) ] 61 | - value(x) = max_u qvalues(x) 62 | - value(x,u) = qvalues(x)[u] 63 | 64 | The model self.trainer corresponds to a mean-square loss of qvalue(x,u) 65 | wrt to a reference q_ref. 66 | The main model self.model has no optimizer and simply computes qvalues,value,qvalue 67 | as a function of x and u (useful for debug only). 68 | Additional helper functions are set to compute the value function and the policy. 69 | ''' 70 | 71 | self.nx=nx;self.nu=nu 72 | input_x = keras.Input(shape=(nx,), name=name+'state') 73 | input_u = keras.Input(shape=(1,), name=name+'control',dtype="int32") 74 | dens1 = keras.layers.Dense(nhiden, activation='relu', name=name+'dense_1', 75 | bias_initializer='random_uniform')(input_x) 76 | dens2 = keras.layers.Dense(nhiden, activation='relu', name=name+'dense_2', 77 | bias_initializer='random_uniform')(dens1) 78 | qvalues = keras.layers.Dense(nu, activation='linear', name=name+'qvalues', 79 | bias_initializer='random_uniform')(dens2) 80 | value = keras.backend.max(qvalues,keepdims=True,axis=1) 81 | value = keras.layers.Lambda(lambda x:x,name=name+'value')(value) 82 | qvalue = batch_gather(qvalues,input_u) 83 | qvalue = keras.layers.Lambda(lambda x:x,name=name+'qvalue')(qvalue) 84 | policy = keras.backend.argmax(qvalues,axis=1) 85 | policy = keras.layers.Lambda(lambda x:x,name=name+'policy')(policy) 86 | 87 | self.trainer = keras.Model(inputs=[input_x,input_u],outputs=qvalue) 88 | self.saver = keras.Model(inputs=input_x,outputs=qvalues) 89 | self.trainer.compile(optimizer='adam',loss='mse') 90 | if learning_rate is not None: 91 | self.trainer.optimizer.lr = learning_rate 92 | 93 | self.model = keras.Model(inputs=[input_x,input_u], 94 | outputs=[qvalues,value,qvalue,policy]) 95 | self.saver = keras.Model(inputs=input_x,outputs=qvalues) # For saving the weights 96 | 97 | self._policy = keras.backend.function(input_x,policy) 98 | self._qvalues = keras.backend.function(input_x,qvalues) 99 | self._value = keras.backend.function(input_x,value) 100 | 101 | # FOR DEBUG ONLY 102 | self._qvalues = keras.backend.function(input_x,qvalues) 103 | self._h1 = keras.backend.function(input_x,dens1) 104 | self._h2 = keras.backend.function(input_x,dens2) 105 | 106 | def targetAssign(self,ref,rate): 107 | ''' 108 | Change model to approach modelRef, with homotopy parameter 109 | (rate=0: do not change, rate=1: exacttly set it to the ref). 110 | ''' 111 | assert(rate<=1 and rate>=0) 112 | for v,vref in zip(self.trainer.trainable_variables,ref.trainer.trainable_variables): 113 | v.assign((1-rate)*v+rate*vref) 114 | 115 | def policy(self,x,noise=None): 116 | ''' 117 | Evaluate the policy u = pi(x) = argmax_u Q(x,u). 118 | If noise is not None, then evaluate a noisy-greedy policy 119 | u = pi(x|noise) = argmax_u(Q(x,u)+uniform(noise)). 120 | ''' 121 | if len(x.shape)==1: x=np.reshape(x,[1,len(x)]) 122 | if noise is None: return self._policy(x) 123 | q = self._qvalues(x) 124 | if noise is not None: q += (np.random.rand(self.nu)*2-1)*noise 125 | return np.argmax(q,axis=1) 126 | 127 | def value(self,x): 128 | ''' 129 | Evaluate the value function at x: V(x). 130 | ''' 131 | if len(x.shape)==1: x=np.reshape(x,[1,len(x)]) 132 | return self._value(x) 133 | 134 | def save(self,filename='qvalue.h5'): 135 | self.saver.save_weights(filename) 136 | def load(self,filename='qvalue.h5'): 137 | self.saver.load_weights(filename) 138 | 139 | 140 | if __name__ == "__main__": 141 | NX = 3; NU = 10 142 | qnet = QNetwork(NX,NU) 143 | 144 | A = np.random.random([ NX,1])*2-1 145 | def data(x): 146 | y = (5*x+3)**2 147 | return y@A 148 | 149 | NSAMPLES = 1000 150 | xs = np.random.random([ NSAMPLES,NX ]) 151 | us = np.random.randint( NU,size=NSAMPLES,dtype=np.int32 ) 152 | ys = np.vstack([ data(x) for x in xs ]) 153 | 154 | qnet.trainer.fit([xs,us],ys,epochs=50,batch_size=64) 155 | 156 | import matplotlib.pylab as plt 157 | plt.ion() 158 | plt.plot(xs,ys, '+') 159 | ypred=qnet.trainer.predict([xs,us]) 160 | plt.plot(xs,ypred, '+r') 161 | -------------------------------------------------------------------------------- /tp4/collision_wrapper.py: -------------------------------------------------------------------------------- 1 | import pinocchio as pin 2 | import numpy as np 3 | 4 | class CollisionWrapper: 5 | def __init__(self,robot,viz=None): 6 | self.robot=robot 7 | self.viz=viz 8 | 9 | self.rmodel = robot.model 10 | self.rdata = self.rmodel.createData() 11 | self.gmodel = self.robot.gmodel 12 | self.gdata = self.gmodel.createData() 13 | self.gdata.collisionRequests.enable_contact = True 14 | 15 | 16 | def computeCollisions(self,q,vq=None): 17 | res = pin.computeCollisions(self.rmodel,self.rdata,self.gmodel,self.gdata,q,False) 18 | pin.computeDistances(self.rmodel,self.rdata,self.gmodel,self.gdata,q) 19 | pin.computeJointJacobians(self.rmodel,self.rdata,q) 20 | if vq is not None: 21 | pin.forwardKinematics(self.rmodel,self.rdata,q,vq,0*vq) 22 | return res 23 | 24 | def getCollisionList(self): 25 | '''Return a list of triplets [ index,collision,result ] where index is the 26 | index of the collision pair, colision is gmodel.collisionPairs[index] 27 | and result is gdata.collisionResults[index]. 28 | ''' 29 | return [ [ir,self.gmodel.collisionPairs[ir],r] 30 | for ir,r in enumerate(self.gdata.collisionResults) if r.isCollision() ] 31 | 32 | def _getCollisionJacobian(self,col,res): 33 | '''Compute the jacobian for one collision only. ''' 34 | contact = res.getContact(0) 35 | g1 = self.gmodel.geometryObjects[col.first] 36 | g2 = self.gmodel.geometryObjects[col.second] 37 | oMc = pin.SE3(pin.Quaternion.FromTwoVectors(np.array([0,0,1]),contact.normal).matrix(),contact.pos) 38 | 39 | joint1 = g1.parentJoint 40 | joint2 = g2.parentJoint 41 | oMj1 = self.rdata.oMi[joint1] 42 | oMj2 = self.rdata.oMi[joint2] 43 | 44 | cMj1 = oMc.inverse()*oMj1 45 | cMj2 = oMc.inverse()*oMj2 46 | 47 | J1=pin.getJointJacobian(self.rmodel,self.rdata,joint1,pin.ReferenceFrame.LOCAL) 48 | J2=pin.getJointJacobian(self.rmodel,self.rdata,joint2,pin.ReferenceFrame.LOCAL) 49 | Jc1=cMj1.action@J1 50 | Jc2=cMj2.action@J2 51 | J = (Jc2-Jc1)[2,:] 52 | return J 53 | 54 | def _getCollisionJdotQdot(self,col,res): 55 | '''Compute the Coriolis acceleration for one collision only. ''' 56 | contact = res.getContact(0) 57 | g1 = self.gmodel.geometryObjects[col.first] 58 | g2 = self.gmodel.geometryObjects[col.second] 59 | oMc = pin.SE3(pin.Quaternion.FromTwoVectors(np.array([0,0,1]),contact.normal).matrix(),contact.pos) 60 | 61 | joint1 = g1.parentJoint 62 | joint2 = g2.parentJoint 63 | oMj1 = self.rdata.oMi[joint1] 64 | oMj2 = self.rdata.oMi[joint2] 65 | 66 | cMj1 = oMc.inverse()*oMj1 67 | cMj2 = oMc.inverse()*oMj2 68 | 69 | a1 = self.rdata.a[joint1] 70 | a2 = self.rdata.a[joint2] 71 | a = (cMj1 * a1 - cMj2 * a2).linear[2] 72 | return a 73 | 74 | def getCollisionJacobian(self,collisions=None): 75 | '''From a collision list, return the Jacobian corresponding to the normal direction. ''' 76 | if collisions is None: collisions = self.getCollisionList() 77 | if len(collisions)==0: return np.ndarray([0,self.rmodel.nv]) 78 | J = np.vstack([ self._getCollisionJacobian(c,r) for (i,c,r) in collisions ]) 79 | return J 80 | 81 | def getCollisionJdotQdot(self,collisions=None): 82 | if collisions is None: 83 | collisions = self.getCollisionList() 84 | if len(collisions)==0: 85 | return np.array([]) 86 | a0 = np.array([self._getCollisionJdotQdot(c,r) for (i,c,r) in collisions]) 87 | return a0 88 | 89 | def getCollisionDistances(self,collisions=None): 90 | if collisions is None: collisions = self.getCollisionList() 91 | if len(collisions)==0: return np.array([]) 92 | dist = np.array([ self.gdata.distanceResults[i].min_distance for (i,c,r) in collisions ]) 93 | return dist 94 | 95 | 96 | # --- DISPLAY ----------------------------------------------------------------------------------- 97 | # --- DISPLAY ----------------------------------------------------------------------------------- 98 | # --- DISPLAY ----------------------------------------------------------------------------------- 99 | 100 | def initDisplay(self,viz=None): 101 | if viz is not None: self.viz = viz 102 | assert(self.viz is not None) 103 | 104 | self.patchName = 'world/contact_%d_%s' 105 | self.ncollisions=10 106 | self.createDisplayPatchs(0) 107 | 108 | def createDisplayPatchs(self,ncollisions): 109 | 110 | if ncollisions == self.ncollisions: return 111 | elif ncollisions0: break 182 | if not i % 20: viz.display(q) 183 | 184 | viz.display(q) 185 | 186 | col.displayCollisions() 187 | p = cols[0][1] 188 | ci = cols[0][2].getContact(0) 189 | 190 | import pickle 191 | with open('/tmp/bug.pickle', 'wb') as file: 192 | pickle.dump([ col.gdata.oMg[11], 193 | col.gdata.oMg[3], 194 | #col.gmodel.geometryObjects[11].geometry, 195 | ] , file) 196 | 197 | dist=col.getCollisionDistances() 198 | J = col.getCollisionJacobian() 199 | -------------------------------------------------------------------------------- /tp0/simple_path_planning.py: -------------------------------------------------------------------------------- 1 | # %jupyter_snippet import 2 | import pinocchio as pin 3 | from utils.meshcat_viewer_wrapper import MeshcatVisualizer 4 | import time 5 | import numpy as np 6 | from numpy.linalg import inv,norm,pinv,svd,eig 7 | from scipy.optimize import fmin_bfgs,fmin_slsqp 8 | from utils.load_ur5_with_obstacles import load_ur5_with_obstacles,Target 9 | import matplotlib.pylab as plt 10 | # %end_jupyter_snippet 11 | plt.ion() # matplotlib with interactive setting 12 | 13 | # %jupyter_snippet robot 14 | robot = load_ur5_with_obstacles(reduced=True) 15 | # %end_jupyter_snippet 16 | 17 | # %jupyter_snippet viewer 18 | viz = MeshcatVisualizer(robot) 19 | viz.display(robot.q0) 20 | # %end_jupyter_snippet 21 | 22 | # %jupyter_snippet target 23 | target = Target(viz,position = np.array([.5,.5])) 24 | # %end_jupyter_snippet 25 | 26 | ################################################################################ 27 | ################################################################################ 28 | ################################################################################ 29 | 30 | # %jupyter_snippet endef 31 | def endef(q): 32 | '''Return the 2d position of the end effector.''' 33 | pin.framesForwardKinematics(robot.model,robot.data,q) 34 | return robot.data.oMf[-1].translation[[0,2]] 35 | # %end_jupyter_snippet 36 | 37 | # %jupyter_snippet dist 38 | def dist(q): 39 | '''Return the distance between the end effector end the target (2d).''' 40 | return norm(endef(q)-target.position) 41 | # %end_jupyter_snippet 42 | 43 | # %jupyter_snippet coll 44 | def coll(q): 45 | '''Return true if in collision, false otherwise.''' 46 | pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) 47 | return pin.computeCollisions(robot.collision_model,robot.collision_data,False) 48 | # %end_jupyter_snippet 49 | 50 | # %jupyter_snippet qrand 51 | def qrand(check=False): 52 | ''' 53 | Return a random configuration. If check is True, this 54 | configuration is not is collision 55 | ''' 56 | while True: 57 | q = np.random.rand(2)*6.4-3.2 # sample between -3.2 and +3.2. 58 | if not check or not coll(q): 59 | return q 60 | # %end_jupyter_snippet 61 | 62 | # %jupyter_snippet colldist 63 | def collisionDistance(q): 64 | '''Return the minimal distance between robot and environment. ''' 65 | pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q) 66 | if pin.computeCollisions(robot.collision_model,robot.collision_data,False): 0 67 | idx = pin.computeDistances(robot.collision_model,robot.collision_data) 68 | return robot.collision_data.distanceResults[idx].min_distance 69 | # %end_jupyter_snippet 70 | ################################################################################ 71 | ################################################################################ 72 | ################################################################################ 73 | 74 | # %jupyter_snippet qrand_target 75 | # Sample a random free configuration where dist is small enough. 76 | def qrandTarget(threshold=5e-2, display=False): 77 | while True: 78 | q = qrand() 79 | if display: 80 | viz.display(q) 81 | time.sleep(1e-3) 82 | if not coll(q) and dist(q)dist(q+dq) and not coll(q+dq): # If distance decrease without collision ... 101 | q = q+dq # ... keep the step 102 | hist.append(q.copy()) # ... keep a trace of it 103 | viz.display(q) # ... display it 104 | time.sleep(5e-3) # ... and sleep for a short while 105 | return hist 106 | # %end_jupyter_snippet 107 | 108 | ################################################################################ 109 | ################################################################################ 110 | ################################################################################ 111 | 112 | # %jupyter_snippet sample 113 | def sampleSpace(nbSamples=500): 114 | ''' 115 | Sample nbSamples configurations and store them in two lists depending 116 | if the configuration is in free space (hfree) or in collision (hcol), along 117 | with the distance to the target and the distance to the obstacles. 118 | ''' 119 | hcol = [] 120 | hfree = [] 121 | for i in range(nbSamples): 122 | q = qrand(False) 123 | if not coll(q): 124 | hfree.append( list(q.flat) + [ dist(q), collisionDistance(q) ]) 125 | else: 126 | hcol.append( list(q.flat) + [ dist(q), 1e-2 ]) 127 | return hcol,hfree 128 | 129 | def plotConfigurationSpace(hcol,hfree,markerSize=20): 130 | ''' 131 | Plot 2 "scatter" plots: the first one plot the distance to the target for 132 | each configuration, the second plots the distance to the obstacles (axis q1,q2, 133 | distance in the color space). 134 | ''' 135 | htotal = hcol + hfree 136 | h=np.array(htotal) 137 | plt.subplot(2,1,1) 138 | plt.scatter(h[:,0],h[:,1],c=h[:,2],s=markerSize,lw=0) 139 | plt.title("Distance to the target") 140 | plt.colorbar() 141 | plt.subplot(2,1,2) 142 | plt.scatter(h[:,0],h[:,1],c=h[:,3],s=markerSize,lw=0) 143 | plt.title("Distance to the obstacles") 144 | plt.colorbar() 145 | 146 | hcol,hfree = sampleSpace(100) 147 | plotConfigurationSpace(hcol,hfree) 148 | # %end_jupyter_snippet 149 | 150 | ################################################################################ 151 | ################################################################################ 152 | ################################################################################ 153 | 154 | ### Plot random trajectories in the same plot 155 | # %jupyter_snippet traj 156 | qinit = np.array([-1.1, -3. ]) 157 | for i in range(100): 158 | traj = randomDescent(qinit) 159 | if dist(traj[-1])<5e-2: 160 | print('We found a good traj!') 161 | break 162 | traj = np.array(traj) 163 | ### Chose trajectory end to be in [-pi,pi] 164 | qend = (traj[-1]+np.pi) % (2*np.pi) - np.pi 165 | ### Take the entire trajectory it modulo 2 pi 166 | traj += (qend-traj[-1]) 167 | # %end_jupyter_snippet 168 | plt.plot(traj[:,0],traj[:,1],'r',lw=5) 169 | 170 | ################################################################################ 171 | ################################################################################ 172 | ################################################################################ 173 | 174 | # %jupyter_snippet optim 175 | def cost(q): 176 | """ 177 | Cost function: distance to the target. 178 | """ 179 | return dist(q) ** 2 180 | 181 | 182 | def constraint(q): 183 | """ 184 | Constraint function: distance to the obstacle should be strictly positive. 185 | """ 186 | min_collision_dist = 0.01 # [m] 187 | return collisionDistance(q) - min_collision_dist 188 | 189 | 190 | def callback(q): 191 | """ 192 | At each optimization step, display the robot configuration. 193 | """ 194 | viz.display(q) 195 | time.sleep(0.01) 196 | 197 | 198 | def optimize(): 199 | ''' 200 | Optimize from an initial random configuration to discover a collision-free 201 | configuration as close as possible to the target. 202 | ''' 203 | return fmin_slsqp(x0=qrand(check=True), 204 | func=cost, 205 | f_ieqcons=constraint,callback=callback, 206 | full_output=1) 207 | optimize() 208 | # %end_jupyter_snippet 209 | 210 | # %jupyter_snippet useit 211 | while True: 212 | res=optimize() 213 | q=res[0] 214 | viz.display(q) 215 | if res[4]=='Optimization terminated successfully' and res[1]<1e-6: 216 | print('Finally successful!') 217 | break 218 | print("Failed ... let's try again! ") 219 | # %end_jupyter_snippet 220 | -------------------------------------------------------------------------------- /tp7/env_pendulum.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Create a simulation environment for a N-pendulum. 3 | See the main at the end of the file for an example. 4 | 5 | We define here 4 main environments that are tested in the __main__: 6 | 7 | - EnvPendulum: state NX=2 continuous, control NU=1 continuous, Euler integration step with DT=1e-2 and high friction 8 | - EnvPendulumDiscrete: state NX=441 discrete, control NU=11 discrete, Euler step DT=0.5 low friction 9 | - EnvPendulumSinCos: state NX=3 with x=[cos,sin,vel], control NU=1 control, Euler step DT=1e-2, high friction 10 | - EnvPendulumHybrid: state NX=3 continuous with x=[cos,sin,vel], control NU=11 discrete, Euler step DT=0.5 low friction 11 | 12 | ''' 13 | 14 | import pinocchio as pin 15 | import numpy as np 16 | from tp7.models.pendulum import createPendulum 17 | from tp7.env_abstract import EnvPinocchio 18 | import tp7.env_abstract as env_abstract 19 | 20 | # --- PENDULUM ND CONTINUOUS -------------------------------------------------------------------- 21 | # --- PENDULUM ND CONTINUOUS -------------------------------------------------------------------- 22 | # --- PENDULUM ND CONTINUOUS -------------------------------------------------------------------- 23 | 24 | class EnvPendulum(EnvPinocchio): 25 | ''' 26 | Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3). 27 | The configuration is nq=7. The velocity is the same. 28 | The members of the class are: 29 | * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them. 30 | * model: the kinematic tree of the robot. 31 | * data: the temporary variables to be used by the kinematic algorithms. 32 | * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being 33 | an object Visual (see above). 34 | 35 | See tp1.py for an example of use. 36 | ''' 37 | 38 | def __init__(self,nbJoint=1,viewer=None): 39 | ''' 40 | Create a Pinocchio model of a N-pendulum, with N the argument . 41 | is expected to be in { "meshcat" | "gepetto" | None }. 42 | ''' 43 | self.model,self.geometryModel = createPendulum(nbJoint) 44 | if viewer == 'meshcat': 45 | from pinocchio.visualize import MeshcatVisualizer 46 | self.viewer = MeshcatVisualizer(self.model,self.geometryModel,self.geometryModel) 47 | self.viewer.initViewer(loadModel=True) 48 | elif 'gepetto' == viewer: 49 | from pinocchio.visualize import GepettoVisualizer 50 | self.viewer = GepettoVisualizer(self.model,self.geometryModel,self.geometryModel) 51 | self.viewer.initViewer(loadModel=True) 52 | else: 53 | self.viewer = None 54 | if viewer is not None: ## warning 55 | print('Error: is expected to be in { "meshcat" | "gepetto" | None }, but we got:', viewer) 56 | 57 | self.q0 = pin.neutral(self.model) 58 | self.v0 = np.zeros(self.model.nv) 59 | self.x0 = np.concatenate([self.q0,self.v0]) 60 | 61 | EnvPinocchio.__init__(self,self.model,self.viewer,taumax=2.5) 62 | self.DT = 1e-2 # duration of one environment step 63 | self.NDT = 5 # number of euler integration step per environment step (i.e integration interval is DT/NDT) 64 | self.Kf = 1.0 # Friction coefficient 65 | 66 | self.costWeights = { 'q': 1, 'v' : 1e-1, 'u' : 1e-3, 'tip' : 0. } 67 | self.tipDes = float(nbJoint) 68 | 69 | def cost(self,x=None,u=None): 70 | if x is None: x = self.x 71 | cost = 0. 72 | q,v = x[:self.nq],x[-self.nv:] 73 | qdes = self.xdes[:self.nq] 74 | cost += self.costWeights['q']*np.sum((q-qdes)**2) 75 | cost += self.costWeights['v']*np.sum(v**2) 76 | cost += 0 if u is None else self.costWeights['u']*np.sum(u**2) 77 | cost += self.costWeights['tip']*(self.tip(q)-self.tipDes)**2 78 | return cost 79 | 80 | def tip(self,q=None): 81 | '''Return the altitude of pendulum tip''' 82 | if q is None: q = self.x[:self.nq] 83 | pin.framesForwardKinematics(self.rmodel,self.rdata,q) 84 | return self.rdata.oMf[1].translation[2] 85 | 86 | def jupyter_cell(self): 87 | return self.viewer.viewer.jupyter_cell() 88 | 89 | # --- SPIN-OFF ---------------------------------------------------------------------------------- 90 | # --- SPIN-OFF ---------------------------------------------------------------------------------- 91 | # --- SPIN-OFF ---------------------------------------------------------------------------------- 92 | 93 | class EnvPendulumDiscrete(env_abstract.EnvDiscretized): 94 | def __init__(self,nbJoint=1,**kwargs): 95 | env = EnvPendulum(nbJoint,**kwargs) 96 | env.DT=5e-1 # Larger integration step to allow larger discretization grid 97 | env.Kf=0.1 # Reduced friction, because larger steps would make friction unstable. 98 | env_abstract.EnvDiscretized.__init__(self,env,21,11) 99 | self.discretize_x.modulo = np.pi*2 100 | self.discretize_x.moduloIdx = range(env.nq) 101 | self.discretize_x.vmax[:env.nq] = np.pi 102 | self.discretize_x.vmin[:env.nq] = -np.pi 103 | self.reset() 104 | self.conti.costWeights = { 'q': 0, 'v' : 0, 'u' : 0, 'tip' : 1 } 105 | self.withSimpleCost = True 106 | def step(self,u): 107 | x,c=env_abstract.EnvDiscretized.step(self,u) 108 | if self.withSimpleCost: 109 | c = int(np.all(np.abs(self.conti.x)<1e-3)) 110 | return x,c 111 | def jupyter_cell(self): 112 | return self.conti.viewer.viewer.jupyter_cell() 113 | 114 | class EnvPendulumSinCos(env_abstract.EnvPartiallyObservable): 115 | def __init__(self,nbJoint=1,**kwargs): 116 | env = EnvPendulum(nbJoint,**kwargs) 117 | def sincos(x,nq): 118 | q,v = x[:nq],x[nq:] 119 | return np.concatenate([np.concatenate([(np.cos(qi),np.sin(qi)) for qi in q]),v]) 120 | def atan(x,nq): 121 | cq,sq,v = x[:2*nq:2],x[1:2*nq:2],x[2*nq:] 122 | return np.concatenate([np.arctan2(sq,cq),v]) 123 | env_abstract.EnvPartiallyObservable.__init__(self,env,nobs=env.nq*2+env.nv, 124 | obs=lambda x:sincos(x,env.nq), 125 | obsInv=lambda csv:atan(csv,env.nq)) 126 | self.reset() 127 | def jupyter_cell(self): 128 | return self.full.viewer.jupyter_cell() 129 | 130 | class EnvPendulumHybrid(env_abstract.EnvDiscretized): 131 | def __init__(self,nbJoint=1,**kwargs): 132 | env = EnvPendulumSinCos(nbJoint,**kwargs) 133 | NU = 21 #11 134 | env_abstract.EnvDiscretized.__init__(self,env,discretize_x=0,discretize_u=NU) 135 | self.conti.full.Kf=.1 # Reduced friction, because larger steps would make friction unstable. 136 | self.conti.full.DT=1e-1 # 5e-1 # Larger integration step to allow larger discretization grid 137 | self.conti.full.costWeights = {'q': 0, 'tip': 1.0, 'u': 0.00, 'v': 0.1 } 138 | 139 | self.reset() 140 | # self.withSimpleCost = False 141 | # def step(self,u): 142 | # x,c=env_abstract.EnvDiscretizedenv_abstract.step(self,u) 143 | # if self.withSimpleCost: 144 | # c = int(np.all(np.abs(self.conti.x)<1e-3)) 145 | # return x,c 146 | def jupyter_cell(self): 147 | return self.conti.full.viewer.viewer.jupyter_cell() 148 | 149 | 150 | # --- MAIN ------------------------------------------------------------------------------- 151 | # --- MAIN ------------------------------------------------------------------------------- 152 | # --- MAIN ------------------------------------------------------------------------------- 153 | 154 | if __name__ == "__main__": 155 | import time 156 | 157 | envs = [] 158 | 159 | env = EnvPendulum(1,viewer='gepetto') 160 | env.name = str(env.__class__) 161 | env.u0 = np.zeros(env.nu) 162 | envs.append(env) 163 | 164 | env = EnvPendulumDiscrete(1,viewer='gepetto') 165 | env.name = str(env.__class__) 166 | env.u0 = env.encode_u(np.zeros(1)) 167 | envs.append(env) 168 | 169 | env = EnvPendulumSinCos(1,viewer='gepetto') 170 | env.name = str(env.__class__) 171 | env.u0 = np.zeros(env.nu) 172 | envs.append(env) 173 | 174 | env = EnvPendulumHybrid(1,viewer='gepetto') 175 | env.name = str(env.__class__) 176 | env.u0 = env.encode_u(np.zeros(1)) 177 | envs.append(env) 178 | 179 | # Reset all environment to the same initial state. 180 | envs[1].reset() 181 | for env in envs: 182 | if env is not envs[1]: env.reset(envs[1].conti.x) 183 | 184 | # Simulate a free fall for all environments. 185 | for env in envs: 186 | env.render() 187 | print(env.name) 188 | time.sleep(1) 189 | for i in range(10): 190 | env.step(env.u0) 191 | env.render() 192 | -------------------------------------------------------------------------------- /B_quaternions.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Quaternions representing rotations SO(3)\n", 8 | "This notebook is a short sumamry of how SO(3) is handled in Pinocchio, and mostly quaternion representation. \n", 9 | "## SO(3), Euler angles, rotations" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "import magic_donotload" 19 | ] 20 | }, 21 | { 22 | "cell_type": "code", 23 | "execution_count": null, 24 | "metadata": {}, 25 | "outputs": [], 26 | "source": [ 27 | "import pinocchio\n", 28 | "from pinocchio import SE3, Quaternion\n", 29 | "import numpy as np\n", 30 | "from numpy.linalg import norm\n", 31 | "M = SE3.Random()\n", 32 | "R = M.rotation\n", 33 | "p = M.translation\n", 34 | "print('R =\\n' + str(R))\n", 35 | "print('p =\\n' + str(p.T))" 36 | ] 37 | }, 38 | { 39 | "cell_type": "markdown", 40 | "metadata": {}, 41 | "source": [ 42 | "A rotation is simply a 3x3 matrix. It has a unit norm:" 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": null, 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "print(R @ R.T)" 52 | ] 53 | }, 54 | { 55 | "cell_type": "markdown", 56 | "metadata": {}, 57 | "source": [ 58 | "It can be equivalently represented by a quaternion. Here we have made the choice to create a specific class for quaternions (i.e. they are not vectors, and can be e.g. multiplied), but you can get the 4 coefficients with the adequate method. Note that the corresponding vector is also of norm 1." 59 | ] 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": null, 64 | "metadata": {}, 65 | "outputs": [], 66 | "source": [ 67 | "quat = Quaternion(R)\n", 68 | "print(norm(quat.coeffs()))" 69 | ] 70 | }, 71 | { 72 | "cell_type": "markdown", 73 | "metadata": {}, 74 | "source": [ 75 | "Angle-axis representation are also implemented in the class AngleAxis." 76 | ] 77 | }, 78 | { 79 | "cell_type": "code", 80 | "execution_count": null, 81 | "metadata": {}, 82 | "outputs": [], 83 | "source": [ 84 | "from pinocchio import AngleAxis\n", 85 | "utheta = AngleAxis(quat)\n", 86 | "print(utheta.angle, utheta.axis)" 87 | ] 88 | }, 89 | { 90 | "cell_type": "markdown", 91 | "metadata": {}, 92 | "source": [ 93 | "You can display rotation in the viewer." 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "from utils.meshcat_viewer_wrapper import MeshcatVisualizer\n", 103 | "viz = MeshcatVisualizer()\n", 104 | "viz.viewer.jupyter_cell()" 105 | ] 106 | }, 107 | { 108 | "cell_type": "code", 109 | "execution_count": null, 110 | "metadata": {}, 111 | "outputs": [], 112 | "source": [ 113 | "viz.addBox('world/box', [.1, .2, .3], [1, 0, 0, 1])\n", 114 | "viz.applyConfiguration('world/box', [.1, .2, .3] + list(quat.coeffs()))" 115 | ] 116 | }, 117 | { 118 | "cell_type": "markdown", 119 | "metadata": {}, 120 | "source": [ 121 | "### Quaternion you said?\n", 122 | "Quaternions are \"complex of complex\", introduced form complex as complex are from reals. Let's try to understand what they contains in practice. Quaternions are of the form w+xi+yj+zk, with w,x,y,z real values, and i,j,k the 3 imaginary numbers. We store them as 4-d vectors, with the real part first: quat = [x,y,z,w]. We can interprete w as encoding the cosinus of the rotation angle. Let's see that." 123 | ] 124 | }, 125 | { 126 | "cell_type": "code", 127 | "execution_count": null, 128 | "metadata": {}, 129 | "outputs": [], 130 | "source": [ 131 | "from numpy import arccos\n", 132 | "print(arccos(quat[3]))\n", 133 | "print(AngleAxis(quat).angle)" 134 | ] 135 | }, 136 | { 137 | "cell_type": "markdown", 138 | "metadata": {}, 139 | "source": [ 140 | "Indeed, w = cos(θ/2). Why divided by two? For that, let's see how the quaternion can be used to represent a rotation. We can encode a 3D vector in the imaginary part of a quaternion." 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "p = np.random.rand(3)\n", 150 | "qp = Quaternion(0., p[0], p[1], p[2])\n", 151 | "print(qp)" 152 | ] 153 | }, 154 | { 155 | "cell_type": "markdown", 156 | "metadata": {}, 157 | "source": [ 158 | "The real product extends over quaternions, so let's try to multiply quat with p:" 159 | ] 160 | }, 161 | { 162 | "cell_type": "code", 163 | "execution_count": null, 164 | "metadata": {}, 165 | "outputs": [], 166 | "source": [ 167 | "print(quat * qp)" 168 | ] 169 | }, 170 | { 171 | "cell_type": "markdown", 172 | "metadata": {}, 173 | "source": [ 174 | "Well that's not a pure imaginary quaternion anymore. And the imaginary part does not contains somethig that looks like the rotated point:" 175 | ] 176 | }, 177 | { 178 | "cell_type": "code", 179 | "execution_count": null, 180 | "metadata": {}, 181 | "outputs": [], 182 | "source": [ 183 | "print(quat.matrix() @ p)" 184 | ] 185 | }, 186 | { 187 | "cell_type": "markdown", 188 | "metadata": {}, 189 | "source": [ 190 | "The pure quaternion is obtained by multiplying again on the left by the conjugate (w,-x,-y,-z)." 191 | ] 192 | }, 193 | { 194 | "cell_type": "code", 195 | "execution_count": null, 196 | "metadata": {}, 197 | "outputs": [], 198 | "source": [ 199 | "print(quat * qp * quat.conjugate())" 200 | ] 201 | }, 202 | { 203 | "cell_type": "markdown", 204 | "metadata": {}, 205 | "source": [ 206 | "That is a pure quaternion, hence encoding a point, and does corresponds to R*p. Magic, is it not? We can prove that the double product of quaternion does corresponds to the rotation. Indeed, a quaternion rather encode an action (a rotation) in $R^4$, but which moves our point p outside of $R^3$. The conjugate rotation brings it back in $R^3$ but applies a second rotation. Since we rotate twice, it is necessary to apply only half of the angle each time.\n", 207 | "What if we try to apply the rotation quat on the imaginary part of the quaternion?" 208 | ] 209 | }, 210 | { 211 | "cell_type": "code", 212 | "execution_count": null, 213 | "metadata": {}, 214 | "outputs": [], 215 | "source": [ 216 | "qim = Quaternion(quat) # copy\n", 217 | "qim[3] = 0\n", 218 | "print(qim, quat * qim * quat.conjugate())" 219 | ] 220 | }, 221 | { 222 | "cell_type": "markdown", 223 | "metadata": {}, 224 | "source": [ 225 | "What kind of conclusion can we get from this? What geometrical interpretation can we give to $q_{im}$? What about $||q_{im}||$?" 226 | ] 227 | }, 228 | { 229 | "cell_type": "markdown", 230 | "metadata": {}, 231 | "source": [ 232 | "### The SLERP example\n", 233 | "Let's practice! Implement a linear interpolation between two position p0 and p1, i.e. find the position p(t) with t varying from 0 to 1, with p(0)=p0, p(1)=p1 and continuity between the two extrema." 234 | ] 235 | }, 236 | { 237 | "cell_type": "code", 238 | "execution_count": null, 239 | "metadata": {}, 240 | "outputs": [], 241 | "source": [ 242 | "# Your code" 243 | ] 244 | }, 245 | { 246 | "cell_type": "markdown", 247 | "metadata": {}, 248 | "source": [ 249 | "And the solution should you need it" 250 | ] 251 | }, 252 | { 253 | "cell_type": "code", 254 | "execution_count": null, 255 | "metadata": {}, 256 | "outputs": [], 257 | "source": [ 258 | "%do_not_load appendix/solution_lerp.py" 259 | ] 260 | }, 261 | { 262 | "cell_type": "markdown", 263 | "metadata": {}, 264 | "source": [ 265 | "LERP with quaternions is not working because they are not normalize. Instead we can take either the normalization of the LERP (NLERP), or the spherical LERP (SLERP). " 266 | ] 267 | }, 268 | { 269 | "cell_type": "code", 270 | "execution_count": null, 271 | "metadata": {}, 272 | "outputs": [], 273 | "source": [ 274 | "# Your code" 275 | ] 276 | }, 277 | { 278 | "cell_type": "markdown", 279 | "metadata": {}, 280 | "source": [ 281 | "And the solution if you need it" 282 | ] 283 | }, 284 | { 285 | "cell_type": "code", 286 | "execution_count": null, 287 | "metadata": {}, 288 | "outputs": [], 289 | "source": [ 290 | "%do_not_load appendix/solution_slerp.py" 291 | ] 292 | }, 293 | { 294 | "cell_type": "markdown", 295 | "metadata": {}, 296 | "source": [ 297 | "## Cheat sheet\n", 298 | "\n", 299 | "You can convert quaternion to rotation matrix and create SE3 objects as follows:\n" 300 | ] 301 | }, 302 | { 303 | "cell_type": "code", 304 | "execution_count": null, 305 | "metadata": {}, 306 | "outputs": [], 307 | "source": [ 308 | "qu = Quaternion(.7,.2,.2,.6)# Quaternion: take care that norm <= 1 (and approx 1)\n", 309 | "R = qu.matrix() # Create a rotation matrix from quaternion\n", 310 | "p = np.array([0.,0.,0.77]) # Translation (R3) vector)\n", 311 | "M = SE3(R,p) # Create a nomogeneous matrix from R,P\n", 312 | "\n", 313 | "# Typical tool position\n", 314 | "from pinocchio.utils import rotate\n", 315 | "M = SE3(rotate('z',1.)@rotate('x',.2), np.array([0.1,0.02,.65]))" 316 | ] 317 | }, 318 | { 319 | "cell_type": "code", 320 | "execution_count": null, 321 | "metadata": {}, 322 | "outputs": [], 323 | "source": [] 324 | } 325 | ], 326 | "metadata": { 327 | "kernelspec": { 328 | "display_name": "Python 3 (ipykernel)", 329 | "language": "python", 330 | "name": "python3" 331 | }, 332 | "language_info": { 333 | "codemirror_mode": { 334 | "name": "ipython", 335 | "version": 3 336 | }, 337 | "file_extension": ".py", 338 | "mimetype": "text/x-python", 339 | "name": "python", 340 | "nbconvert_exporter": "python", 341 | "pygments_lexer": "ipython3", 342 | "version": "3.9.13" 343 | } 344 | }, 345 | "nbformat": 4, 346 | "nbformat_minor": 4 347 | } 348 | -------------------------------------------------------------------------------- /tp7/env_abstract.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Base (abstract) class for environments. 3 | ''' 4 | import numpy as np 5 | import time 6 | from tp7.discretization import VectorDiscretization 7 | 8 | def scalarOrArrayToArray(x,nx): 9 | return x if isinstance(x,np.ndarray) else np.array([x,]*nx,np.float64) 10 | 11 | # ---------------------------------------------------------------------------------------- 12 | # --- Env Abstract ----------------------------------------------------------------------- 13 | # ---------------------------------------------------------------------------------------- 14 | class EnvAbstract: 15 | ''' 16 | Base (abstract) class for environments. 17 | ''' 18 | def __init__(self,nx,nu): 19 | ''' 20 | ''' 21 | self.nx = nx 22 | self.nu = nu 23 | 24 | def render(self): 25 | ''' 26 | Call self.display(self.x). 27 | ''' 28 | self.display(self.x) 29 | 30 | def reset(self,x=None): 31 | ''' 32 | This method internally calls self.randomState() and stores the results. 33 | ''' 34 | if x is None: self.x = self.randomState() 35 | else: self.x = x.copy() if isinstance(x,np.ndarray) else x 36 | return self.x 37 | 38 | def step(self,u): 39 | ''' 40 | This methods calls self.x,self.cost = self.dynAndCost(self.x,u). 41 | Modifies the internal state self.x. 42 | ''' 43 | self.x,cost = self.dynAndCost(self.x,u) 44 | return self.x,-cost 45 | 46 | # Internal methods corresponding to reset (randomState), step (cost and dyn) and 47 | # render (display). They are all to be used with continuous state and control, and are 48 | # read only (no change of internal state). 49 | def randomState(self): 50 | ''' 51 | Returns a random state x. 52 | Const method, dont modify the environment (i.e env.x is not modified). 53 | ''' 54 | assert(False and "This method should be implemented by inheritance.") 55 | 56 | def dynAndCost(self,x,u): 57 | ''' 58 | Considering a control u applied at current state x, the method returns 59 | (xnext,cost), where xnext is the next state xnext = f(x,u), and cost 60 | is the cost for making this action cost=l(x,u). 61 | This method is called inside step(u). 62 | Const method, dont modify the environment (i.e env.x is not modified). 63 | ''' 64 | assert(False and "This method should be implemented by inheritance.") 65 | 66 | def display(self,x): 67 | ''' 68 | Display the argument state x (not the internal env.x). 69 | This method is called inside env.render(). 70 | Const method, dont modify the environment (i.e env.x is not modified). 71 | ''' 72 | assert(False and "This method should be implemented by inheritance.") 73 | 74 | 75 | # ---------------------------------------------------------------------------------------- 76 | # --- CONTINUOUS ENV --------------------------------------------------------------------- 77 | # ---------------------------------------------------------------------------------------- 78 | class EnvContinuousAbstract(EnvAbstract): 79 | def __init__(self,nx,nu,xmax=10.,xmin=None,umax=1.,umin=None): 80 | EnvAbstract.__init__(self,nx,nu) 81 | self.xmax = scalarOrArrayToArray(xmax,self.nx) 82 | self.xmin = scalarOrArrayToArray(xmin,self.nx) if xmin is not None else -self.xmax 83 | self.umax = scalarOrArrayToArray(umax,self.nu) 84 | self.umin = scalarOrArrayToArray(umin,self.nu) if umin is not None else -self.umax 85 | self.xspan = self.xmax-self.xmin 86 | self.uspan = self.umax-self.umin 87 | 88 | def randomState(self): 89 | return self.xmin+np.random.random(self.nx)*self.xspan 90 | def randomControl(self): 91 | return self.umin+np.random.random(self.nu)*self.uspan 92 | 93 | import pinocchio as pin 94 | class EnvPinocchio(EnvContinuousAbstract): 95 | def __init__(self,pinocchioModel,viewer=None,taumax=1.0): 96 | self.rmodel = pinocchioModel 97 | self.rdata = self.rmodel.createData() 98 | self.nq = self.rmodel.nq 99 | self.nv = self.rmodel.nv 100 | qmax = self.rmodel.upperPositionLimit 101 | qmin = self.rmodel.lowerPositionLimit 102 | vmax = self.rmodel.velocityLimit 103 | vmin = -vmax 104 | self.viewer = viewer 105 | EnvContinuousAbstract.__init__(self,nx=self.nq+self.nv,nu=self.nv, 106 | xmax=np.concatenate([qmax.ravel(),vmax.ravel()]), 107 | xmin=np.concatenate([qmin.ravel(),vmin.ravel()]), 108 | umax=taumax) 109 | # Options parameters 110 | self.sleepAtDisplay = 8e-2 111 | self.xdes = np.zeros(self.nx) 112 | self.costWeights = { 'wx': 1., 'wu': 1e-2 } 113 | self.DT = 5e-2 # Step length 114 | self.NDT = 2 # Number of Euler steps per integration (internal) 115 | self.Kf = .1 # Friction coefficient (remove it with Kf=0). 116 | self.reset() 117 | 118 | def randomState(self): 119 | #q = pin.randomConfiguration(self.rmodel) 120 | dq = self.xmax[:self.nq]-self.xmin[:self.nq] 121 | q = np.random.random(self.nq)*dq + self.xmin[:self.nq] 122 | dv = self.xmax[-self.nv:]-self.xmin[-self.nv:] 123 | v = np.random.random(self.nv)*dv+self.xmin[-self.nv:] 124 | return np.concatenate([q,v]) 125 | def display(self,x,sleep=None): 126 | if sleep is not None: self.sleepAtDisplay = sleep 127 | q,v = x[:self.nq],x[-self.nv:] 128 | if self.viewer is not None: 129 | self.viewer.display(q) 130 | time.sleep(self.sleepAtDisplay) 131 | def cost(self,x,u): 132 | '''Default cost function.''' 133 | cost = 0. 134 | cost += self.costWeights['wx']*np.sum((x-self.xdes)**2) 135 | cost += self.costWeights['wu']*np.sum(u**2) 136 | return cost 137 | def dynAndCost(self,x,u,verbose=False): 138 | x=x.copy() 139 | q,v = x[:self.nq],x[-self.nv:] 140 | u = np.clip(u,-self.umax,self.umax) 141 | cost = 0. 142 | dt = self.DT/self.NDT 143 | for t in range(self.NDT): 144 | # Add friction 145 | tau = u.copy() 146 | if self.Kf>0.0: tau -= self.Kf*v 147 | # Evaluate cost 148 | cost += self.cost(np.concatenate([q,v]),u)/self.NDT 149 | #print(self,self.cost,t,x,u,cost) 150 | # Evaluate dynamics 151 | a = pin.aba(self.rmodel,self.rdata,q,v,tau) 152 | if verbose: print(q,v,tau,a) 153 | v += a*dt 154 | v = np.clip(v,self.xmin[self.nq:],self.xmax[self.nq:]) 155 | q = pin.integrate(self.rmodel,q,v*dt) 156 | xnext = np.concatenate([q,v]) 157 | return xnext,cost 158 | 159 | # ---------------------------------------------------------------------------------------- 160 | # --- PARTIALLY OBSERVABLE --------------------------------------------------------------- 161 | # ---------------------------------------------------------------------------------------- 162 | class EnvPartiallyObservable(EnvContinuousAbstract): 163 | def __init__(self,env_fully_observable,nobs,obs,obsInv=None): 164 | ''' 165 | Define a partially-observable markov model from a fully-observable model 166 | and an observation function. 167 | The new env model is defined with the observable as new state, while 168 | the original state is kept inside the fully-observable model. 169 | 170 | @param env_fully_observable: the fully-observable model. 171 | @param obs: the observation function y=h(x) 172 | @param obsinv: if available, the inverse function of h: x=h^-1(y). 173 | ''' 174 | self.full = env_fully_observable 175 | self.obs = obs 176 | self.obsinv = obsInv 177 | EnvContinuousAbstract.__init__(self,nx=nobs, 178 | nu=self.full.nu, 179 | xmax=obs(self.full.xmax), 180 | xmin=obs(self.full.xmin), 181 | umax=self.full.umax,umin=self.full.umin) 182 | 183 | def randomState(self): 184 | return self.obs(self.full.randomState()) 185 | def dynAndCost(self,x,u): 186 | assert(self.obsinv is not None) 187 | x,c = self.full.dynAndCost(self.obsinv(x),u) 188 | return self.obs(x),c 189 | def display(self,x): 190 | assert(self.obsinv is not None) 191 | self.full.display(self.obsinv(x)) 192 | 193 | def reset(self,x=None): 194 | assert(x is not None or self.obsinv is not None) 195 | return self.obs(self.full.reset(x)) 196 | def step(self,u): 197 | x,c = self.full.step(u) 198 | return self.obs(x),c 199 | def render(self): 200 | return self.full.render() 201 | @property 202 | def x(self): 203 | return self.obs(self.full.x) 204 | 205 | 206 | 207 | # ---------------------------------------------------------------------------------------- 208 | # --- DISCRETIZED ENV -------------------------------------------------------------------- 209 | # ---------------------------------------------------------------------------------------- 210 | 211 | class EnvDiscretized(EnvAbstract): 212 | def __init__(self,envContinuous,discretize_x = 0,discretize_u = 0): 213 | self.conti = envContinuous 214 | if discretize_u != 0: 215 | self.discretize_u = VectorDiscretization(self.conti.nu, 216 | vmax=self.conti.umax,nsteps=discretize_u) 217 | self.encode_u = self.discretize_u.c2i 218 | self.decode_u = self.discretize_u.i2c 219 | nu = self.discretize_u.nd 220 | else: 221 | self.discretize_u = None 222 | self.encode_u = lambda u:u 223 | self.decode_u = lambda u:u 224 | nu = envContinuous.nu 225 | if discretize_x != 0: 226 | self.discretize_x = VectorDiscretization(self.conti.nx, 227 | vmax=self.conti.xmax,nsteps=discretize_x) 228 | self.encode_x = self.discretize_x.c2i 229 | self.decode_x = self.discretize_x.i2c 230 | nx = self.discretize_x.nd 231 | else: 232 | self.discretize_x = None 233 | self.encode_x = lambda x:x 234 | self.decode_x = lambda x:x 235 | nx = envContinuous.nx 236 | 237 | EnvAbstract.__init__(self,nx=nx,nu=nu) 238 | 239 | def randomState(self): 240 | return self.encode_x(self.conti.randomState()) 241 | def display(self,x): 242 | self.conti.display(self.decode_x(x)) 243 | def dynAndCost(self,x,u): 244 | x,c=self.conti.dynAndCost(self.decode_x(x),self.decode_u(u)) 245 | return self.encode_x(x),c 246 | def reset(self,xi=None): 247 | if xi is None: 248 | x = self.conti.reset() 249 | xi = self.encode_x(x) 250 | else: 251 | x = None 252 | x_eps = self.decode_x(xi) 253 | if x_eps is not x: 254 | self.conti.reset(x_eps) 255 | self.x = xi 256 | return self.x 257 | def step(self,u): 258 | ###assert(self.x == self.encode_x(self.conti.x)) 259 | ###assert(np.allclose(self.decode_x(self.x),self.conti.x)) 260 | x,c = self.conti.step(self.decode_u(u)) 261 | self.x= self.encode_x(x) 262 | x_eps = self.decode_x(self.x) 263 | if x_eps is not x: self.conti.reset(x_eps) 264 | return self.x,c 265 | def render(self): 266 | self.conti.render() 267 | 268 | 269 | 270 | 271 | --------------------------------------------------------------------------------