From 2465a5dd5f3b636b0a02ba033d626ff5a2791e9c Mon Sep 17 00:00:00 2001 From: Kamal Karki Date: Sat, 26 Sep 2026 14:22:54 +0530 Subject: [PATCH] chore(examples): fix broken scripts, remove dead ones, add README index Part of #677 (examples folder rationalization). - icra2021.py, mexican-wave.py: update to the current ET/Robot/xplot/ iscollided API. - Remove fetch_vision.py (FetchCamera gone since #545), ikine_evaluate2.py (ikine_min never existed), and test.py (scratch file). - Add examples/README.md indexing every script with its required extras. --- examples/README.md | 91 +++++++++++++ examples/fetch_vision.py | 227 ------------------------------- examples/icra2021.py | 22 +-- examples/ikine_evaluate2.py | 94 ------------- examples/mexican-wave.py | 2 +- examples/test.py | 262 ------------------------------------ 6 files changed, 103 insertions(+), 595 deletions(-) create mode 100644 examples/README.md delete mode 100644 examples/fetch_vision.py delete mode 100644 examples/ikine_evaluate2.py delete mode 100644 examples/test.py diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 000000000..2533aaee0 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,91 @@ +# Examples + +Standalone scripts that demonstrate the toolbox. They are not run by the test +suite; run one directly from the repository root, for example: + +```shell +python examples/puma_jtraj.py +``` + +## Requirements + +All scripts need the base install, `pip install roboticstoolbox-python`. +Where a script needs an optional extra it is listed in the table: + +| Extra | Install | Provides | +|---|---|---| +| `swift` | `pip install roboticstoolbox-python[swift]` | the browser-based Swift visualiser | +| `qp` | `pip install roboticstoolbox-python[qp]` | `qpsolvers` and `quadprog` for the QP-based controllers | +| `collision` | `pip install roboticstoolbox-python[collision]` | `coal` and `trimesh` for collision checking. `coal` has no Windows build, so use Linux, macOS or WSL | + +`pip install roboticstoolbox-python[all]` installs every extra at once. + +## Getting started and plotting + +| Script | What it shows | Extras | +|---|---|---| +| `readme.py` | The code from the top-level README: DH Panda, IK, a joint trajectory, Swift animation | swift | +| `plot.py` | DH Panda animated through random joint angles with velocity and force ellipsoids, matplotlib backend | | +| `plot_swift.py` | Minimal: load the URDF Panda and plot it | swift | +| `puma_swift.py` | URDF Puma 560 interpolating between its named configurations in Swift | swift | +| `robots.py` | Loads a dozen URDF models side by side in Swift | swift | +| `teach.py` | Interactive teach pendant on the DH Panda, matplotlib backend | | +| `teach_swift.py` | Hand-rolled Swift teach panel; a template for custom slider UIs | swift | +| `mexican-wave.py` | Fifteen cloned Pumas doing a wave in Swift | swift | + +## Trajectories and dynamics + +| Script | What it shows | Extras | +|---|---|---| +| `puma_jtraj.py` | Joint-space trajectory (`jtraj`) between Puma configurations; `--backend` and `--model` options | | +| `puma_fdyn.py` | Forward dynamics (`fdyn`) of the Puma under zero torque, plotted and animated | | + +## Resolved-rate and QP control + +| Script | What it shows | Extras | +|---|---|---| +| `RRMC.py` | Resolved-rate motion control on the DH Panda, matplotlib backend | | +| `RRMC_swift.py` | The same controller on the URDF Panda in Swift | swift | +| `park.py` | Null-space manipulability maximisation (Park 1999) | swift | +| `baur.py` | As `park.py` with an added joint-limit avoidance term (Baur 2012) | swift | +| `mmc.py` | Manipulability-maximising QP controller (Haviland and Corke 2020) | swift, qp | +| `swift_recording.py` | `mmc.py` with Swift video recording enabled | swift, qp | +| `neo.py` | NEO reactive obstacle avoidance using `link_collision_damper` | swift, qp, collision | + +## Mobile manipulation + +| Script | What it shows | Extras | +|---|---|---| +| `holistic_mm_omni.py` | Holistic mobile manipulation with an omnidirectional base (FrankieOmni) | swift, qp | +| `holistic_mm_non_holonomic.py` | The same with a non-holonomic base. Currently broken: `rtb.models.Frankie()` no longer includes a mobile base | swift, qp | + +## Branched robots + +| Script | What it shows | Extras | +|---|---|---| +| `branched_robot.py` | Two-arm YuMi controlled with one ETS per gripper | swift | + +## Paper walkthroughs + +| Script | What it shows | Extras | +|---|---|---| +| `icra2021.py` | The code listings from the ICRA 2021 paper "Not your grandmother's toolbox" | swift, collision | + +## Benchmarks and checks + +| Script | What it shows | Extras | +|---|---|---| +| `benchmark_ik.py` | Wall time per problem for the IK solvers, C++ versus pure Python. See the wiki page Benchmark-IK | | +| `benchmark_rne.py` | Correctness cross-check and timing of the `rne` implementations. See the wiki page Benchmark-RNE | | +| `_cpu_info.py` | Helper used by the two benchmark scripts; not runnable on its own | | +| `ik_exp.py` | Success rate and iteration counts of the IK solvers over random targets | | +| `ikine_evaluate.py` | Timing of analytic and numerical IK on the DH Puma and Panda | | +| `rne_compare.py` | Puma 560: C versus Python `rne`, and the DH-convention guard in `Robot.rne` | | +| `rne_dh_convention_check.py` | One-link DH and MDH gravity torque against a Lagrangian ground truth | | + +## Mobile robots + +| Script | What it shows | Extras | +|---|---|---| +| `mobile.py` | Snippets from RVC chapter 5; mostly commented out | | +| `vehicle1.py` | A `VehicleIcon` animation; mostly commented out | | diff --git a/examples/fetch_vision.py b/examples/fetch_vision.py deleted file mode 100644 index 335b2cdd6..000000000 --- a/examples/fetch_vision.py +++ /dev/null @@ -1,227 +0,0 @@ -#!/usr/bin/env python -""" -@author Kerry He and Rhys Newbury -""" - -import swift -import spatialgeometry as sg -import roboticstoolbox as rtb -import spatialmath as sm -import numpy as np -import qpsolvers as qp -import math - - -def transform_between_vectors(a, b): - # Finds the shortest rotation between two vectors using angle-axis, - # then outputs it as a 4x4 transformation matrix - a = a / np.linalg.norm(a) - b = b / np.linalg.norm(b) - - angle = np.arccos(np.dot(a, b)) - axis = np.cross(a, b) - - return sm.SE3.AngleAxis(angle, axis) - - -# Launch the simulator Swift -env = swift.Swift() -env.launch() - -# Create a Fetch and Camera robot object -fetch = rtb.models.Fetch() -fetch_camera = rtb.models.FetchCamera() - -# Set joint angles to zero configuration -fetch.q = fetch.qz -fetch_camera.q = fetch_camera.qz - -# Make target object obstacles with velocities -target = sg.Sphere(radius=0.05, base=sm.SE3(-2.0, 0.0, 0.5)) - -# Make line of sight object to visualize where the camera is looking -sight_base = sm.SE3.Ry(np.pi / 2) * sm.SE3(0.0, 0.0, 2.5) -centroid_sight = sg.Cylinder( - radius=0.001, - length=5.0, - base=fetch_camera.fkine(fetch_camera.q).A @ sight_base.A, -) - -# Add the Fetch and other shapes to the simulator -env.add(fetch) -env.add(fetch_camera) -env.add(centroid_sight) -env.add(target) - -# Set the desired end-effector pose to the location of target -Tep = fetch.fkine(fetch.q) -Tep.A[:3, :3] = sm.SE3.Rz(np.pi).R -Tep.A[:3, 3] = target.T[:3, -1] - -env.step() - -n_base = 2 -n_arm = 8 -n_camera = 2 -n = n_base + n_arm + n_camera - - -def step(): - - # Find end-effector pose in world frame - wTe = fetch.fkine(fetch.q).A - # Find camera pose in world frame - wTc = fetch_camera.fkine(fetch_camera.q).A - - # Find transform between end-effector and goal - eTep = np.linalg.inv(wTe) @ Tep.A - # Find transform between camera and goal - cTep = np.linalg.inv(wTc) @ Tep.A - - # Spatial error between end-effector and target - et = np.sum(np.abs(eTep[:3, -1])) - - # Weighting function used for objective function - def w_lambda(et, alpha, gamma): - return alpha * np.power(et, gamma) - - # Quadratic component of objective function - Q = np.eye(n + 10) - - Q[: n_base + n_arm, : n_base + n_arm] *= 0.01 # Robotic manipulator - Q[:n_base, :n_base] *= w_lambda(et, 1.0, -1.0) # Mobile base - Q[n_base + n_arm : n, n_base + n_arm : n] *= 0.01 # Camera - Q[n : n + 3, n : n + 3] *= w_lambda(et, 1000.0, -2.0) # Slack arm linear - Q[n + 3 : n + 6, n + 3 : n + 6] *= w_lambda(et, 0.01, -5.0) # Slack arm angular - Q[n + 6 : -1, n + 6 : -1] *= 100 # Slack camera - Q[-1, -1] *= w_lambda(et, 1000.0, 3.0) # Slack self-occlusion - - # Calculate target velocities for end-effector to reach target - v_manip, _ = rtb.p_servo(wTe, Tep, 1.5) - v_manip[3:] *= 1.3 - - # Calculate target angular velocity for camera to rotate towards target - head_rotation = transform_between_vectors(np.array([1, 0, 0]), cTep[:3, 3]) - v_camera, _ = rtb.p_servo(sm.SE3(), head_rotation, 25) - - # The equality contraints to achieve velocity targets - Aeq = np.c_[fetch.jacobe(fetch.q), np.zeros((6, 2)), np.eye(6), np.zeros((6, 4))] - beq = v_manip.reshape((6,)) - - jacobe_cam = fetch_camera.jacobe(fetch_camera.q)[3:, :] - Aeq_cam = np.c_[ - jacobe_cam[:, :3], - np.zeros((3, 7)), - jacobe_cam[:, 3:], - np.zeros((3, 6)), - np.eye(3), - np.zeros((3, 1)), - ] - Aeq = np.r_[Aeq, Aeq_cam] - beq = np.r_[beq, v_camera[3:].reshape((3,))] - - # The inequality constraints for joint limit avoidance - Ain = np.zeros((n + 10, n + 10)) - bin = np.zeros(n + 10) - - # The minimum angle (in radians) in which the joint is allowed to approach - # to its limit - ps = 0.1 - - # The influence angle (in radians) in which the velocity damper - # becomes active - pi = 0.9 - - # Form the joint limit velocity damper - Ain[: fetch.n, : fetch.n], bin[: fetch.n] = fetch.joint_velocity_damper( - ps=ps, pi=pi, n=fetch.n - ) - - Ain_torso, bin_torso = fetch_camera.joint_velocity_damper( - ps=0.0, pi=0.05, n=fetch_camera.n - ) - Ain[2, 2] = Ain_torso[2, 2] - bin[2] = bin_torso[2] - - Ain_cam, bin_cam = fetch_camera.joint_velocity_damper(ps=ps, pi=pi, n=fetch_camera.n) - Ain[n_base + n_arm : n, n_base + n_arm : n] = Ain_cam[3:, 3:] - bin[n_base + n_arm : n] = bin_cam[3:] - - # Create line of sight object between camera and object - c_Ain, c_bin = fetch.vision_collision_damper( - target, - camera=fetch_camera, - camera_n=2, - q=fetch.q[: fetch.n], - di=0.3, - ds=0.2, - xi=1.0, - end=fetch.link_dict["gripper_link"], - start=fetch.link_dict["shoulder_pan_link"], - ) - - if c_Ain is not None and c_bin is not None: - c_Ain = np.c_[ - c_Ain, np.zeros((c_Ain.shape[0], 9)), -np.ones((c_Ain.shape[0], 1)) - ] - - Ain = np.r_[Ain, c_Ain] - bin = np.r_[bin, c_bin] - - # Linear component of objective function: the manipulability Jacobian - c = np.concatenate( - ( - np.zeros(n_base), - # -fetch.jacobm(start=fetch.links[3]).reshape((n_arm,)), - np.zeros(n_arm), - np.zeros(n_camera), - np.zeros(10), - ) - ) - - # Get base to face end-effector - kε = 0.5 - bTe = fetch.fkine(fetch.q, include_base=False).A - θε = math.atan2(bTe[1, -1], bTe[0, -1]) - ε = kε * θε - c[0] = -ε - - # The lower and upper bounds on the joint velocity and slack variable - lb = -np.r_[ - fetch.qdlim[: fetch.n], - fetch_camera.qdlim[3 : fetch_camera.n], - 100 * np.ones(9), - 0, - ] - ub = np.r_[ - fetch.qdlim[: fetch.n], - fetch_camera.qdlim[3 : fetch_camera.n], - 100 * np.ones(9), - 100, - ] - - # Solve for the joint velocities dq - qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub) - qd_cam = np.concatenate((qd[:3], qd[fetch.n : fetch.n + 2])) - qd = qd[: fetch.n] - - if et > 0.5: - qd *= 0.7 / et - qd_cam *= 0.7 / et - else: - qd *= 1.4 - qd_cam *= 1.4 - - arrived = et < 0.02 - - fetch.qd = qd - fetch_camera.qd = qd_cam - centroid_sight.T = fetch_camera.fkine(fetch_camera.q).A @ sight_base.A - - return arrived - - -arrived = False -while not arrived: - arrived = step() - env.step(0.01) diff --git a/examples/icra2021.py b/examples/icra2021.py index 2c6295ca9..fccddf238 100644 --- a/examples/icra2021.py +++ b/examples/icra2021.py @@ -7,7 +7,7 @@ from swift import Swift import spatialmath.base.symbolic as sym -from roboticstoolbox import ETS as ET +from roboticstoolbox import ET from roboticstoolbox import * from spatialmath import * from spatialgeometry import * @@ -77,20 +77,20 @@ e = ( ET.tz(l1) - * ET.rz() + * ET.Rz() * ET.ty(l2) - * ET.ry() + * ET.Ry() * ET.tz(l3) * ET.tx(l4) * ET.ty(l5) - * ET.ry() + * ET.Ry() * ET.tz(l6) - * ET.rz() - * ET.ry() - * ET.rz() + * ET.Rz() + * ET.Ry() + * ET.Rz() ) -robot = ERobot(e) +robot = Robot(e) print(robot) panda = models.URDF.Panda() @@ -100,7 +100,7 @@ # ## B. Trajectories traj = jtraj(puma.qz, puma.qr, 100) -qplot(traj.q) +xplot(traj.q) t = np.arange(0, 2, 0.010) T0 = SE3(0.6, -0.5, 0.3) @@ -158,8 +158,8 @@ obstacle = Box([1, 1, 1], base=SE3(1, 0, 0)) -iscollision0 = panda.collided(panda.q, obstacle) # boolean -iscollision1 = panda.links[0].collided(obstacle) +iscollision0 = panda.iscollided(panda.q, obstacle) # boolean +iscollision1 = panda.links[0].iscollided(obstacle) d, p1, p2 = panda.closest_point(panda.q, obstacle) print(d, p1, p2) diff --git a/examples/ikine_evaluate2.py b/examples/ikine_evaluate2.py deleted file mode 100644 index f6aee2532..000000000 --- a/examples/ikine_evaluate2.py +++ /dev/null @@ -1,94 +0,0 @@ -import numpy as np -from spatialmath import SE3 -import roboticstoolbox as rtb -import timeit -from ansitable import ANSITable, Column -import traceback - -# change for the robot IK under test, must set: -# * robot, the DHRobot object -# * T, the end-effector pose -# * q0, the initial joint angles for solution - -example = "puma" # 'panda' - -if example == "puma": - # Puma robot case - robot = rtb.models.DH.Puma560() - q = robot.qn - q0 = robot.qz - T = robot.fkine(q) -elif example == "panda": - # Panda robot case - robot = rtb.models.DH.Panda() - T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) - q0 = robot.qz - -solvers = [ - "Nelder-Mead", - "Powell", - "CG", - "BFGS", - "Newton-CG", ## Jacobian is required - "L-BFGS-B", - "TNC", - "COBYLA", - "SLSQP", - "trust-constr", - "dogleg", - "trust-ncg", - "trust-exact", - "trust-krylov", -] - - -# setup to run timeit -setup = """ -from __main__ import robot, T, q0 -""" -N = 10 - -# setup results table -table = ANSITable( - Column("Solver", headalign="^", colalign="<"), - Column("Time (ms)", headalign="^", fmt="{:.2g}", colalign=">"), - Column("Error", headalign="^", fmt="{:.3g}", colalign=">"), - border="thick", -) - -# test the IK methods -for solver in solvers: - print("Testing:", solver) - - # test the method, don't pass q0 to the analytic function - try: - sol = robot.ikine_min(T, q0=q0, qlim=True, method=solver) - except Exception as e: - print("***", solver, " failed") - print(e) - continue - - # print error message if there is one - if not sol.success: - print(" failed:", sol.reason) - - # evalute the error - err = np.linalg.norm(T - robot.fkine(sol.q)) - print(" error", err) - - if N > 0: # noqa - # evaluate the execution time - t = timeit.timeit( - stmt=f"robot.ikine_min(T, q0=q0, qlim=True, method='{solver}')", - setup=setup, - number=N, - ) - else: - t = 0 - - # add it to the output table - table.row(f"`{solver}`", t / N * 1e3, err) - -# pretty print the results -table.print() -print(table.markdown()) diff --git a/examples/mexican-wave.py b/examples/mexican-wave.py index 971ee07ef..a01a9d068 100644 --- a/examples/mexican-wave.py +++ b/examples/mexican-wave.py @@ -25,7 +25,7 @@ base = SE3.Rz(theta) * SE3(2, 0, 0) # Clone the robot - puma = rtb.ERobot(puma0) + puma = rtb.Robot(puma0) puma.base = base puma.q = puma0.qz env.add(puma) diff --git a/examples/test.py b/examples/test.py deleted file mode 100644 index 0f258f575..000000000 --- a/examples/test.py +++ /dev/null @@ -1,262 +0,0 @@ -import numpy as np -import roboticstoolbox as rtb -import spatialmath as sm - - -def rne(robot, q, qd, qdd): - n = len(robot.links) - - # allocate intermediate variables - Xup = sm.SE3.Alloc(n) - Xtree = sm.SE3.Alloc(n) - - # The body velocities, accelerations and forces - v = sm.SpatialVelocity.Alloc(n) - a = sm.SpatialAcceleration.Alloc(n) - f = sm.SpatialForce.Alloc(n) - - # The joint inertia - I = sm.SpatialInertia.Alloc(n) # noqa: E741 - - # Joint motion subspace matrix - s = [] - - q = robot.qr - qd = np.zeros(n) - qdd = np.zeros(n) - - Q = np.zeros(n) - - for i, link in enumerate(robot.links): - # Allocate the inertia - I[i] = sm.SpatialInertia(link.m, link.r, link.I) - - # Compute the link transform - Xtree[i] = sm.SE3(link.Ts, check=False) # type: ignore - - # Append the variable axis of the joint to s - if link.v is not None: - s.append(link.v.s) - else: - s.append(np.zeros(6)) - - a_grav = -sm.SpatialAcceleration([0, 0, 9.81]) - - # Forward recursion - for i, link in enumerate(robot.links): - if link.jindex is None: - qi = 0 - qdi = 0 - qddi = 0 - else: - qi = q[link.jindex] - qdi = qd[link.jindex] - qddi = qdd[link.jindex] - - vJ = sm.SpatialVelocity(s[i] * qdi) - - # Transform from parent(j) to j - if link.isjoint: - Xup[i] = sm.SE3(link.A(qi)).inv() - else: - Xup[i] = sm.SE3(link.A()).inv() - - if link.parent is None: - v[i] = vJ - a[i] = Xup[i] * a_grav + sm.SpatialAcceleration(s[i] * qddi) - else: - v[i] = Xup[i] * v[i - 1] + vJ - a[i] = Xup[i] * a[i - 1] + sm.SpatialAcceleration(s[i] * qddi) + v[i] @ vJ - - f[i] = I[i] * a[i] + v[i] @ (I[i] * v[i]) - - # backward recursion - for i in reversed(range(n)): - link = robot.links[i] - - Q[i] = sum(f[i].A * s[i]) - - if link.parent is not None: - f[i - 1] = f[i - 1] + Xup[i] * f[i] - - return Q - - -def rne_eff(robot, q, qd, qdd): - n = len(robot.links) - - # allocate intermediate variables - Xup = np.empty((n, 4, 4)) - - Xtree = np.empty((n, 4, 4)) - - # The body velocities, accelerations and forces - v = sm.SpatialVelocity.Alloc(n) - a = sm.SpatialAcceleration.Alloc(n) - f = sm.SpatialForce.Alloc(n) - - # The joint inertia - I = sm.SpatialInertia.Alloc(n) # noqa: E741 - - # Joint motion subspace matrix - s = [] - - q = robot.qr - qd = np.zeros(n) - qdd = np.zeros(n) - - Q = np.zeros(n) - - for i, link in enumerate(robot.links): - # Allocate the inertia - I[i] = sm.SpatialInertia(link.m, link.r, link.I) - - # Compute the link transform - Xtree[i] = link.Ts - - # Append the variable axis of the joint to s - if link.v is not None: - s.append(link.v.s) - else: - s.append(np.zeros(6)) - - a_grav = -sm.SpatialAcceleration([0, 0, 9.81]) - - # Forward recursion - for i, link in enumerate(robot.links): - if link.jindex is None: - qi = 0 - qdi = 0 - qddi = 0 - else: - qi = q[link.jindex] - qdi = qd[link.jindex] - qddi = qdd[link.jindex] - - vJ = sm.SpatialVelocity(s[i] * qdi) - - # Transform from parent(j) to j - if link.isjoint: - Xup[i] = np.linalg.inv(link.A(qi)) - else: - Xup[i] = np.linalg.inv(link.A()) - - if link.parent is None: - v[i] = vJ - a[i] = Xup[i] * a_grav + sm.SpatialAcceleration(s[i] * qddi) - else: - v[i] = Xup[i] * v[i - 1] + vJ - a[i] = Xup[i] * a[i - 1] + sm.SpatialAcceleration(s[i] * qddi) + v[i] @ vJ - - f[i] = I[i] * a[i] + v[i] @ (I[i] * v[i]) - - # backward recursion - for i in reversed(range(n)): - link = robot.links[i] - - Q[i] = sum(f[i].A * s[i]) - - if link.parent is not None: - f[i - 1] = f[i - 1] + Xup[i] * f[i] - - return Q - - -robot = rtb.models.KinovaGen3() - -glink = rtb.Link( - rtb.ET.tz(0.12), - name="gripper_link", - m=0.831, - r=[0, 0, 0.0473], - parent=robot.links[-1], -) - -robot.links.append(glink) - -# for link in robot.links: -# print() -# print(link.name) -# print(link.isjoint) -# print(link.m) -# print(link.r) -# print(link.I) - -# q = rne(robot, robot.qr, np.zeros(7), np.zeros(7)) -# q = robot.qr -# qd = np.zeros(7) -# qdd = np.zeros(7) - -# for i in range(1000): -# tau = robot.rne(q, qd, qdd) -# # q = rne(robot, robot.qr, np.zeros(7), np.zeros(7)) -# # print(i) - - -# print(np.round(tau, 2)) - -# [ 0. 0. 14.2 0.12 -9.22 0. -4.47 -0. 0. 0. 0. ] - -l1a = rtb.Link(ets=rtb.ETS(rtb.ET.Rx()), m=1, r=[0.5, 0, 0], name="l1") -l2a = rtb.Link(ets=rtb.ETS(rtb.ET.Rz()), m=1, r=[0, 0.5, 0], parent=l1a, name="l2") -l3a = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.0, 0, 0.5], parent=l2a, name="l3") -robota = rtb.Robot([l1a, l2a, l3a], name="simple 3 link a") - -l1b = rtb.Link(ets=rtb.ETS(rtb.ET.Rx()), m=1, r=[0.5, 0, 0], name="l1") -l2b = rtb.Link(ets=rtb.ETS(rtb.ET.Rz(0.5)), m=1, r=[0, 0.5, 0], parent=l1b, name="l2") -l3b = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.0, 0, 0.5], parent=l2b, name="l3") -robotb = rtb.Robot([l1b, l2b, l3b], name="simple 3 link b") - -l1c = rtb.Link(ets=rtb.ETS(rtb.ET.tx(0.5)), m=1, r=[0.5, 0, 0], name="l1") -l2c = rtb.Link(ets=rtb.ETS(rtb.ET.Rx()), m=1, r=[0, 0.5, 0], parent=l1c, name="l2") - -# Branch 1 -l3c = rtb.Link(ets=rtb.ETS(rtb.ET.tz(0.5)), m=1, r=[0.0, 0, 0.5], parent=l2c, name="l3") -l4c = rtb.Link(ets=rtb.ETS(rtb.ET.Rz()), m=1, r=[0.0, 0, 0.5], parent=l3c, name="l4") - -# Branch 2 -l5c = rtb.Link(ets=rtb.ETS(rtb.ET.tz(0.5)), m=1, r=[0.0, 0, 0.5], parent=l2c, name="l5") -l6c = rtb.Link(ets=rtb.ETS(rtb.ET.Rz()), m=1, r=[0.0, 0, 0.5], parent=l5c, name="l6") - -robotc = rtb.Robot([l1c, l2c, l3c, l4c, l5c, l6c], name="branch 3 link c") - -za = np.array([0.5, 0.5, 0.5]) -zb = np.array([0.5, 0.5]) -zc = np.array([0.5, 0.5, 0.5]) - -print("\nRobot A") -taua = robota.rne(za, za, za) -print(taua) - -print("\nRobot B") -taub = robotb.rne(zb, zb, zb) -print(taub) - -print("\nRobot C") -tauc = robotc.rne(zc, zc, zc) -print(tauc) - - -# l1 = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.5, 0, 0], name="l1") -# l2 = rtb.Link(ets=rtb.ETS(rtb.ET.tx(1)), m=1, r=[0.5, 0, 0], parent=l1, name="l2") -# l3 = rtb.Link(ets=rtb.ETS([rtb.ET.Ry()]), m=0, r=[0.0, 0, 0], parent=l2, name="l3") -# robot = rtb.Robot([l1, l2, l3], name="simple 3 link") -# z = np.zeros(robot.n) - -# # check gravity load -# tau = robot.rne(z, z, z) / 9.81 -# print(tau) - -# # nt.assert_array_almost_equal(tau, np.r_[-2, -0.5]) - -# print("\n\nNew Robot\n") -# l1 = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.5, 0, 0], name="l1") -# l2 = rtb.Link( -# ets=rtb.ETS([rtb.ET.tx(1), rtb.ET.Ry()]), m=1, r=[0.5, 0, 0], parent=l1, name="l2" -# ) -# robot = rtb.Robot([l1, l2], name="simple 2 link") -# z = np.zeros(robot.n) - -# # check gravity load -# tau = robot.rne(z, z, z) / 9.81 -# print(tau)