def go_down(connection): servo_position = get_position(connection) time.sleep(1) N1 = int(servo_position[0]) head_position = get_head_position(connection, servo_pos=servo_position) rbot = rtb.models.DH.widowx() pos = [ head_position[0], 0, head_position[1] + 36 ] # [x,y,z] = [l, 0, 36+h], 0 mais ballec faut juste pas commander le servo 1 pas = 20 dt = 0.31 H = np.flip(np.append(np.arange(-50, pos[2], pas), pos[2])) viapt = [] ang_reel = [ servo_position[0] * (pi / 2048), (servo_position[1] - 1024) * (pi / 2048), (servo_position[2] - 1024) * (pi / 2048) ] # [theta_1, theta_2, theta_3], angles réels du robot ang = [ang_reel[0], pi - 0.3443 - ang_reel[1], ang_reel[2] + 0.3443 - pi ] # [phi_1, phi_2, phi_3], angles utilisés par le modèle for h in H: T = SE3(pos[0], pos[1], h) * SE3.OA([0, 1, 0], [0, 0, -1]) sol = rbot.ikinem(T, q0=np.array( ([ang[0]], [ang[1]], [ang[2]])).T) # ikinem: cinématique inverse numérique, viapt.append(sol[0]) ang = [sol[0][0], sol[0][1], sol[0][2]] VIA = np.array(viapt) qt = rtb.trajectory.mstraj( VIA, dt, tacc=0.2, qdmax=10 ) # tacc : temps d'accélération | qdmax : vitesse max | dt : delay # rbot.plot(qt.q, movie='panda1.gif') # plot de la trajectoire traj = qt.q print("trajectoire :" + str(len(traj))) for i in range(len(traj)): ang2_mod = traj[i][1] ang3_mod = traj[i][2] ang2_reel = pi - 0.3443 - ang2_mod ang3_reel = ang3_mod - 0.3443 + pi N2_pile = (2048 / pi) * ang2_reel + 1024 N3_pile = (2048 / pi) * ang3_reel + 1024 N2 = int((2048 / pi) * ang2_reel + 1024) N3 = int((2048 / pi) * ang3_reel + 1024) N4 = int(1024 + N2_pile - N3_pile)
# this is the example code from the t0p-level README..d from spatialmath import SE3 import roboticstoolbox as rtb import swift robot = rtb.models.DH.Panda() print(robot) T = robot.fkine(robot.qz) print(T) # IK T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) sol = robot.ikine_LMS(T) # solve IK, ignore additional outputs print(sol.q) # display joint angles # FK shows that desired end-effector pose was achieved print(robot.fkine(sol.q)) qtraj = rtb.jtraj(robot.qz, sol.q, 50) robot.plot(qtraj.q, movie="panda1.gif") # URDF + Swift version dt = 0.050 # simulation timestep in seconds robot = rtb.models.URDF.Panda() print(robot) env = swift.Swift() # instantiate 3D browser-based visualizer env.launch("chrome") # activate it env.add(robot) # add robot to the 3D scene env.start_recording("panda2", 1 / dt)
Psi = np.arccos( (a2**2 - d4**2 - a3**2 + V114**2 + Pz**2) / (2.0 * a2 * r)) if np.isnan(Psi): theta = [] else: theta[1] = np.arctan2(Pz, V114) + n2 * Psi # Solve for theta[2] # theta[2] uses equation 57, p. 40. num = np.cos(theta[1]) * V114 + np.sin(theta[1]) * Pz - a2 den = np.cos(theta[1]) * Pz - np.sin(theta[1]) * V114 theta[2] = np.arctan2(a3, d4) - np.arctan2(num, den) return theta def ikine_a(self, T, config="lun"): return self.ikine_6s(T, config, self._ikine) if __name__ == '__main__': # pragma nocover puma = Puma560(symbolic=False) print(puma) T = SE3(0.5, 0.2, 0.5) * SE3.OA([0, 0, 1], [1, 0, 0]) (q, failed, reason) = puma.ikine(T) print(failed, q) qq = puma.ikine_a(T) print(qq) print(puma.fkine(qq))
def chomp(mode, seed=0, num_pts=1, nq=10): np.random.seed(seed) assert mode in MODES print(f"CHOMP Mode {mode}") robot = rtb.models.DH.Panda() # load Mesh version (for collisions) # robot = rtb.models.URDF.Panda() # load URDF version of the Panda (for visuals) # print(robot) T = robot.fkine(robot.qz) # forward kinematics T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) sol = robot.ikine_LM(T) # solve IK # print(sol) q_pickup = sol.q # print(robot.fkine(q_pickup)) # FK shows that desired end-effector pose was achieved qtraj = rtb.jtraj(robot.qz, q_pickup, 50) # robot.plot(qt.q, movie='panda1.gif') robot = rtb.models.URDF.Panda() # load URDF version of the Panda obstacle = Box([0.3, 0.3, 0.3], base=SE3(0.5, 0.5, 0.8)) pt = robot.closest_point(robot.q, obstacle) meshes = {} if use_mesh: for link in robot.links: if len(link.geometry) != 1: continue kwargs = trimesh.exchange.dae.load_collada( link.geometry[0].filename) # kwargs = trimesh.exchange.dae.load_collada(filename) mesh = trimesh.exchange.load.load_kwargs(kwargs) meshes[link.name] = mesh.dump(concatenate=True) if verbose: print(link.name, mesh) # Hyperparameters dt = 1 lmbda = 1000 eta = 1000 iters = 4 # Make cost field, starting & end points cdim = len(qtraj.q[0]) xidim = cdim * nq AA = np.zeros((xidim, xidim)) xi = np.zeros(xidim) # robot._set_link_fk(qtraj.q[1]) qs = np.zeros(cdim) qe = q_pickup bb = np.zeros(xidim) bb[:cdim] = qs bb[-cdim:] = qe xi[:cdim] = qs bb /= -dt * dt * (nq + 1) # Make a matrix for i in range(nq): AA[cdim * i:cdim * (i + 1), cdim * i:cdim * (i + 1)] = 2 * np.eye(cdim) if i > 0: AA[cdim * (i - 1):cdim * i, cdim * i:cdim * (i + 1)] = -1 * np.eye(cdim) AA[cdim * i:cdim * (i + 1), cdim * (i - 1):cdim * i] = -1 * np.eye(cdim) AA /= dt * dt * (nq + 1) Ainv = np.linalg.pinv(AA) for t in range(iters): with Profiler(f"Mode {mode} iteration"): nabla_smooth = AA.dot(xi) + bb nabla_obs = np.zeros(xidim) xidd = AA.dot(xi) total_cost = 0 pts_all = [] for il, link in enumerate(robot.links): k = link.jindex if k is None: continue mesh = meshes[link.name] idxs = np.random.choice(np.arange(len(mesh.vertices)), num_pts, replace=False) pts = mesh.vertices[idxs] pts_all.append(pts) if mode == "TS": with Profiler(f"Before", enable=verbose): link_base_all = [] pts_xyz_all = [] qd_all = [] jacobs2s = [] robot_idxs = {} pts_all_ = [] for i in range(nq): # timestep qt = xi[cdim * i:cdim * (i + 1)] if i == 0: qd = 0.5 * (xi[cdim * (i + 1):cdim * (i + 2)] - qs) elif i == nq - 1: qd = 0.5 * (qe - xi[cdim * (i - 1):cdim * (i)]) else: qd = 0.5 * (xi[cdim * (i + 1):cdim * (i + 2)] - xi[cdim * (i - 1):cdim * (i)]) qd_all.append(qd) for il, link in enumerate(robot.links): # bodypart k = link.jindex if k is None: continue link_base = robot.fkine( qt, end=link) # x_current, (4, 4) link_base_all.append(link_base.A) delta_nabla_obs = np.zeros(k + 1) pts_se3 = vrepeat(np.eye(4), num_pts) pts = pts_all[k] pts_all_.append(pts) pts_se3[:, :3, 3] = pts pts_xyz = link_base.A.dot(pts_se3).swapaxes( 0, 1)[:, :3, 3] pts_xyz_all.append(pts_xyz) link_base_all = np.stack( link_base_all) # (nq * cdim, 4, 4) # import pdb; pdb.set_trace() pts_all_ = np.concatenate( pts_all_) # (nq * cdim * num_pts, 3) pts_xyz_all = np.concatenate( pts_xyz_all) # (nq * cdim * num_pts, 3) qd_all = np.stack(qd_all) # (nq, cdim) path, njoints, _ = robot.get_path() assert njoints == cdim jacobs = np.zeros((len(pts_all_), 6, cdim)) # (nq * cdim * num_pts, 6, cdim) with Profiler(f"Main", enable=verbose): jacob0_vec(robot, nq, cdim, link_base_all, pts_all_, jacobs, xi) # IMPORTANT: pass xi with Profiler(f"After", enable=verbose): jacobs = jacobs.reshape(nq, cdim * num_pts, 6, cdim) jacobs_ = jacobs.reshape( nq, cdim, num_pts, 6, cdim) # (nq, cdim, num_pts, 6, cdim) xd = vmmatmul(jacobs_, qd_all).reshape( -1, 6) # x_delta, (nq * cdim * num_pts, 6) jacobs = jacobs_.reshape( -1, 6, cdim) # (nq * cdim * num_pts, 6, cdim) vel = np.linalg.norm( xd, axis=-1, keepdims=True) # (nq * cdim * npoints, 1) xdn = xd / (vel + 1e-3 ) # speed normalized, (nq * cdim * num_pts, 6) xidd_ = xidd.reshape(nq, cdim) # (nq, cdim) xdd = vmmatmul(jacobs_, xidd_).reshape( -1, 6) # second derivative of xi, (nq * cdim * num_pts, 6) prj = vrepeat(np.eye(6), nq * cdim * num_pts) #import pdb; pdb.set_trace() prj -= np.matmul( xdn[:, :, None], xdn[:, None, :] ) # curvature vector (nq * cdim * num_pts, 6, 6) kappa = (vmatmul(prj, xdd) / (vel**2 + 1e-3) ) # (nq * cdim * num_pts, 6) cost = np.sum(pts_xyz_all, axis=1, keepdims=True) # (nq * cdim * num_pts, 1) delta = -1 * np.concatenate([[1, 1, 0], np.zeros(3)]) delta = vrepeat(delta, nq * cdim * num_pts) # (nq * cdim * num_pts, 6) part1 = jacobs.swapaxes( 1, 2) # (nq * cdim * num_pts, cdim, 6) part2 = vmatmul( prj, delta) - cost * kappa # (nq * cdim * num_pts, 6) delta_nabla_obs = vmatmul( part1, part2) * vel # (nq * cdim * num_pts, cdim) total_cost += cost.mean() * cdim * nq delta_nabla_obs = delta_nabla_obs.reshape( nq, cdim, num_pts, -1) # import pdb; pdb.set_trace() nabla_obs = delta_nabla_obs.mean(axis=(1, 2)).flatten() * cdim else: for i in range(nq): # timestep qt = xi[cdim * i:cdim * (i + 1)] # qt = qe if i == 0: qd = 0.5 * (xi[cdim * (i + 1):cdim * (i + 2)] - qs) elif i == nq - 1: qd = 0.5 * (qe - xi[cdim * (i - 1):cdim * (i)]) else: qd = 0.5 * (xi[cdim * (i + 1):cdim * (i + 2)] - xi[cdim * (i - 1):cdim * (i)]) if mode == "LINKS": ## Concate all links together with Profiler("Before", enable=verbose): link_base_t = [] pts_t = [] pts_xyz_t = [] for il, link in enumerate(robot.links): # bodypart k = link.jindex if k is None: continue link_base = robot.fkine( qt, end=link) # x_current, (4, 4) link_base_t.append(link_base.A) pts = pts_all[k] pts_t.append(pts) pts_se3 = vrepeat(np.eye(4), num_pts) pts_se3[:, :3, 3] = pts pts_xyz = link_base.A.dot(pts_se3).swapaxes( 0, 1)[:, :3, 3] pts_xyz_t.append(pts_xyz) link_base_t = np.stack(link_base_t) # (cdim, 4, 4) pts_t = np.concatenate( pts_t) # (cdim * npoints, 3) pts_xyz_t = np.concatenate(pts_xyz_t) path, njoints, _ = robot.get_path() jacobs = np.zeros((len(pts_t), 6, njoints)) with Profiler("Main", enable=verbose): jacob0_vec(robot, 1, njoints, link_base_t, pts_t, jacobs, qt) with Profiler("After", enable=verbose): xd = jacobs.dot(qd) # x_delta, (N, 6) vel = np.linalg.norm(xd, axis=1, keepdims=True) # (N, 1) xdn = xd / (vel + 1e-3) # speed normalized, (N, 6) xdd = jacobs.dot( xidd[cdim * i:cdim * (i + 1)]) # second derivative of xi, (N, 6) prj = vrepeat(np.eye(6), num_pts * njoints) prj -= np.matmul( xdn[:, :, None], xdn[:, None, :]) # curvature vector (N, 6, 6) kappa = (vmatmul(prj, xdd) / (vel**2 + 1e-3) ) # (N, 6) cost = np.sum(pts_xyz_t, axis=1, keepdims=True) # (N, 1) delta = -1 * np.concatenate([[1, 1, 0], np.zeros(3)]) delta = vrepeat(delta, num_pts * njoints) # (N, 3, 6) part1 = jacobs.swapaxes(1, 2) # (N, cdim, 6) part2 = vmatmul(prj, delta) - cost * kappa # (N, 6) delta_nabla_obs = vmatmul(part1, part2) * vel # (N, cdim) total_cost += cost.mean() * njoints nabla_obs[cdim * i:cdim * (i + 1)] = delta_nabla_obs.mean( axis=0) * njoints else: ## Calculate links in for loop for il, link in enumerate(robot.links): # bodypart k = link.jindex if k is None: continue with Profiler("Before", enable=verbose): link_base = robot.fkine( qt, end=link) # x_current, (4, 4) delta_nabla_obs = np.zeros(k + 1) pts = pts_all[k] pts_se3 = vrepeat(np.eye(4), num_pts) pts_se3[:, :3, 3] = pts pts_xyz = link_base.A.dot(pts_se3).swapaxes( 0, 1)[:, :3, 3] with Profiler("Main", enable=verbose): path, njoints, _ = robot.get_path(end=link) jacobs = np.zeros((num_pts, 6, njoints)) if mode == "PTS": jacob0_pts_vec(robot, link, pts, jacobs, qt) else: jacob0_pts_loop(robot, link, pts, jacobs, qt) # (N, 6, k + 1) with Profiler("After", enable=verbose): xd = jacobs.dot(qd[:k + 1]) # x_delta, (N, 6) vel = np.linalg.norm(xd, axis=1, keepdims=True) # (N, 1) xdn = xd / (vel + 1e-3 ) # speed normalized, (N, 6) xdd = jacobs.dot( xidd[cdim * i:cdim * i + k + 1]) # second derivative of xi, (N, 6) prj = vrepeat(np.eye(6), num_pts) prj -= np.matmul( xdn[:, :, None], xdn[:, None, :]) # curvature vector (N, 6, 6) kappa = (vmatmul(prj, xdd) / (vel**2 + 1e-3) ) # (N, 6) cost = np.sum(pts_xyz, axis=1, keepdims=True) # (N, 1) # delta := negative gradient of obstacle cost in work space, (6, cdim) delta = -1 * np.concatenate([[1, 1, 0], np.zeros(3)]) delta = vrepeat(delta, num_pts) # (N, 3, 6) # for link in robot.links: # cost = get_link_cost(robot, meshes, link) part1 = jacobs.swapaxes(1, 2) # (N, k + 1, 6) part2 = vmatmul(prj, delta) - cost * kappa # (N, 6) delta_nabla_obs = vmatmul( part1, part2) * vel # (N, k + 1) total_cost += cost.mean() nabla_obs[cdim * i:cdim * i + k + 1] += delta_nabla_obs.mean(axis=0) # dxi = Ainv.dot(lmbda * nabla_smooth) dxi = Ainv.dot(nabla_obs + lmbda * nabla_smooth) xi -= dxi / eta if verbose: print(f"Iteration {t} total cost {total_cost}") if visualize: from roboticstoolbox.backends.swift import Swift # instantiate 3D browser-based visualizer backend = Swift() backend.launch() # activate it robot.q = qs backend.add(robot) # add robot to the 3D scene backend.add(obstacle) backend.step() print("start") dt = 0.07 marker = None marker_pos = None for i in range(nq): if verbose: print(i, xi[cdim * i:cdim * (i + 1)]) robot.q = xi[cdim * i:cdim * (i + 1)] # update the robot state if i == 0: link = robot.links[3] mesh = meshes[link.name] marker_pos = mesh.vertices[0] tool = SE3(marker_pos) xx_base = robot.fkine(robot.q, end=link) # x_current, (4, 4) xx = xx_base @ tool # xx = robot.fkine(robot.q, end=link, tool=tool) # x_current, (4, 4) # xx = get_link_cost(robot, meshes, link) marker = Sphere(0.02, base=SE3(xx)) backend.add(marker) backend.step() else: tool = SE3(marker_pos) # xx = robot.fkine(robot.q, end=link, tool=tool) # x_current, (4, 4) xx_base = robot.fkine(robot.q, end=link) # x_current, (4, 4) xx = xx_base @ tool # xx = get_link_cost(robot, meshes, link) marker.base = SE3(xx) backend.step() # update visualization time.sleep(dt) robot.q = qe backend.step() time.sleep(dt) backend.close()
def test_parallel(seed=0): np.random.seed(seed) robot = rtb.models.DH.Panda() # load Mesh version (for collisions) T = robot.fkine(robot.qz) # forward kinematics T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) sol = robot.ikine_LM(T) # solve IK q_pickup = sol.q robot = rtb.models.URDF.Panda() # load URDF version of the Panda meshes = {} for link in robot.links: if len(link.geometry) != 1: # print(len(link.geometry)) continue kwargs = trimesh.exchange.dae.load_collada(link.geometry[0].filename) # kwargs = trimesh.exchange.dae.load_collada(filename) mesh = trimesh.exchange.load.load_kwargs(kwargs) meshes[link.name] = mesh.dump(concatenate=True) print(link.name, mesh) # Hyperparameters dt = 1 nq = 50 lmbda = 1000 eta = 1000 iters = 4 num_pts = 2000 # Make cost field, starting & end points cdim = len(robot.q) xidim = cdim * nq AA = np.zeros((xidim, xidim)) xi = np.zeros(xidim) # for idx in [2, 3, 4, 5, 6, 7]: for idx in [2]: print(f"Iidx {idx}") link = robot.links[idx] k = link.jindex # Non-parallel, use for loop mesh = meshes[link.name] idxs = np.random.choice(np.arange(len(mesh.vertices)), num_pts, replace=False) pts = mesh.vertices[idxs] path, njoints, _ = robot.get_path(end=link) t_start = time.time() jacobs1 = np.zeros((num_pts, 6, njoints)) jacob0_pts_loop(robot, link, pts, jacobs1, q_pickup) print(f"\nNon-parallel time: {time.time() - t_start:.3f}\n") # print(jacobs1) # Parallel, use cuda t_start = time.time() jacobs2 = np.zeros((num_pts, 6, njoints)) jacob0_pts_vec(robot, link, pts, jacobs2, q_pickup, verbose=True) print(f"\nParallel time: {time.time() - t_start:.3f}\n") print(jacobs2) # print(f"Max diff 0 {np.max(np.abs(jacobs1[0] - jacobs2[0]))}") # print(f"Max diff 0 {np.argmax(np.abs(jacobs1 - jacobs2))}") # print(f"Max diff {np.max(np.abs(jacobs1 - jacobs2))}") # import pdb; pdb.set_trace() assert np.all(np.isclose(jacobs1, jacobs2))
def test_constructor(self): # null constructor R = SE3() nt.assert_equal(len(R), 1) array_compare(R, np.eye(4)) self.assertIsInstance(R, SE3) # construct from matrix R = SE3(trotx(0.2)) nt.assert_equal(len(R), 1) array_compare(R, trotx(0.2)) self.assertIsInstance(R, SE3) # construct from canonic rotation R = SE3.Rx(0.2) nt.assert_equal(len(R), 1) array_compare(R, trotx(0.2)) self.assertIsInstance(R, SE3) R = SE3.Ry(0.2) nt.assert_equal(len(R), 1) array_compare(R, troty(0.2)) self.assertIsInstance(R, SE3) R = SE3.Rz(0.2) nt.assert_equal(len(R), 1) array_compare(R, trotz(0.2)) self.assertIsInstance(R, SE3) # construct from canonic translation R = SE3.Tx(0.2) nt.assert_equal(len(R), 1) array_compare(R, transl(0.2, 0, 0)) self.assertIsInstance(R, SE3) R = SE3.Ty(0.2) nt.assert_equal(len(R), 1) array_compare(R, transl(0, 0.2, 0)) self.assertIsInstance(R, SE3) R = SE3.Tz(0.2) nt.assert_equal(len(R), 1) array_compare(R, transl(0, 0, 0.2)) self.assertIsInstance(R, SE3) # triple angle R = SE3.Eul([0.1, 0.2, 0.3]) nt.assert_equal(len(R), 1) array_compare(R, eul2tr([0.1, 0.2, 0.3])) self.assertIsInstance(R, SE3) R = SE3.Eul(np.r_[0.1, 0.2, 0.3]) nt.assert_equal(len(R), 1) array_compare(R, eul2tr([0.1, 0.2, 0.3])) self.assertIsInstance(R, SE3) R = SE3.Eul([10, 20, 30], unit='deg') nt.assert_equal(len(R), 1) array_compare(R, eul2tr([10, 20, 30], unit='deg')) self.assertIsInstance(R, SE3) R = SE3.RPY([0.1, 0.2, 0.3]) nt.assert_equal(len(R), 1) array_compare(R, rpy2tr([0.1, 0.2, 0.3])) self.assertIsInstance(R, SE3) R = SE3.RPY(np.r_[0.1, 0.2, 0.3]) nt.assert_equal(len(R), 1) array_compare(R, rpy2tr([0.1, 0.2, 0.3])) self.assertIsInstance(R, SE3) R = SE3.RPY([10, 20, 30], unit='deg') nt.assert_equal(len(R), 1) array_compare(R, rpy2tr([10, 20, 30], unit='deg')) self.assertIsInstance(R, SE3) R = SE3.RPY([0.1, 0.2, 0.3], order='xyz') nt.assert_equal(len(R), 1) array_compare(R, rpy2tr([0.1, 0.2, 0.3], order='xyz')) self.assertIsInstance(R, SE3) # angvec R = SE3.AngVec(0.2, [1, 0, 0]) nt.assert_equal(len(R), 1) array_compare(R, trotx(0.2)) self.assertIsInstance(R, SE3) R = SE3.AngVec(0.3, [0, 1, 0]) nt.assert_equal(len(R), 1) array_compare(R, troty(0.3)) self.assertIsInstance(R, SE3) # OA R = SE3.OA([0, 1, 0], [0, 0, 1]) nt.assert_equal(len(R), 1) array_compare(R, np.eye(4)) self.assertIsInstance(R, SE3) # random R = SE3.Rand() nt.assert_equal(len(R), 1) self.assertIsInstance(R, SE3) # random T = SE3.Rand() R = T.R t = T.t T = SE3.Rt(R, t) self.assertIsInstance(T, SE3) self.assertEqual(T.A.shape, (4, 4)) nt.assert_equal(T.R, R) nt.assert_equal(T.t, t) # copy constructor R = SE3.Rx(pi / 2) R2 = SE3(R) R = SE3.Ry(pi / 2) array_compare(R2, trotx(pi / 2)) # SO3 T = SE3(SO3()) nt.assert_equal(len(T), 1) self.assertIsInstance(T, SE3) nt.assert_equal(T.A, np.eye(4)) # SE2 T = SE3(SE2(1, 2, 0.4)) nt.assert_equal(len(T), 1) self.assertIsInstance(T, SE3) self.assertEqual(T.A.shape, (4, 4)) nt.assert_equal(T.t, [1, 2, 0])