From 56369634506c6c22b08854569039745de550140d Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Wed, 31 Jan 2024 15:37:37 +0100 Subject: [PATCH 1/7] add example to visualize floating base velocity --- examples/floating-base-velocity-viewer.py | 92 +++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 examples/floating-base-velocity-viewer.py diff --git a/examples/floating-base-velocity-viewer.py b/examples/floating-base-velocity-viewer.py new file mode 100644 index 0000000000..a3bdfde390 --- /dev/null +++ b/examples/floating-base-velocity-viewer.py @@ -0,0 +1,92 @@ +import pinocchio as pin +import numpy as np +import hppfcl + +from pinocchio.visualize import MeshcatVisualizer +from time import sleep + +def create_pin_cube_model(j0='freeflyer'): + model = pin.Model() + + if j0 == 'freeflyer': + j0 = pin.JointModelFreeFlyer() + elif j0 == 'spherical': + j0 = pin.JointModelSpherical() + elif j0 == 'sphericalzyx': + j0 = pin.JointModelSphericalZYX() + else: + raise ValueError("Unknown joint type") + + jointCube = model.addJoint(0, j0, pin.SE3.Identity(), "joint0") + M = pin.SE3(np.eye(3), np.matrix([0.0, 0.0, 0.0]).T) + model.appendBodyToJoint(jointCube, pin.Inertia.FromBox(1, 0.8, 0.4, 0.2), M) + return model + +def create_pin_geometry_cube_model(model): + jointCube = model.getFrameId("joint0") + geom_model = pin.GeometryModel() + cube_shape = hppfcl.Box(0.8, 0.4, 0.2) # x, y, z + cube = pin.GeometryObject( + "cube_shape", 0, jointCube, cube_shape, pin.SE3.Identity() + ) + cube.meshColor = np.array([1.0, 0.1, 0.1, 0.5]) + geom_model.addGeometryObject(cube) + return geom_model + +def create_model(joint0_type='freeflyer'): + model = create_pin_cube_model(joint0_type) + geom_model = create_pin_geometry_cube_model(model) + return model, geom_model + +def pin_step(model, vizer, v_index_increment, dt = 0.1, enable_gravity=True): + + if not enable_gravity: + model.gravity.setZero() + + data = model.createData() + + tau = np.zeros(model.nv) + q = pin.neutral(model) + v = np.zeros(model.nv) + v[v_index_increment] += 1. + + vizer.display(q) + print(f"{q=}, {v=}") + + sleep(1) + + a = pin.aba(model, data, q, v, tau) + v_next = v + a * dt + q_next = pin.integrate(model, q, v_next * dt) + + print(f"{a=}, {q_next=}, {v_next=}") + return q_next, v_next + + +if __name__ == "__main__": + + list_joint0 = ['freeflyer', 'sphericalzyx', 'spherical'] + + for joint0_name in list_joint0: + + print(f"\njoint0_name = {joint0_name}") + + model, geom_model = create_model(joint0_name) + print(f"{model.nq=}") + print(f"{model.nv=}") + + q = pin.neutral(model) + print(f"neutral q configuration: {q=}") + + vizer = MeshcatVisualizer(model, geom_model, geom_model) + vizer.initViewer(open=True, loadModel=True) + vizer.display(q) + + sleep(2) + + for i in range(model.nv): + print(f"v[{i}] += 1") + q_next, v_next = pin_step(model, vizer, i, dt=0.2, enable_gravity=False) + vizer.display(q_next) + sleep(2) + From 7ed869fb0a1557cf743605677bd61a518a77f254 Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Wed, 31 Jan 2024 15:55:50 +0100 Subject: [PATCH 2/7] new entry to changelog for adding example --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 52f4bfef3e..7aa478bbdb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Fixed - Remove a lot of warnings ([#2139](https://github.com/stack-of-tasks/pinocchio/pull/2139)) +### Added +- Add example to visualize floating base velocity [#2143](https://github.com/stack-of-tasks/pinocchio/pull/2143) + ## [2.7.0] - 2024-01-23 ### Added From 1c94651485f2f0da4c0980f513a09ab6d8bfffa2 Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Fri, 2 Feb 2024 10:59:42 +0100 Subject: [PATCH 3/7] Update CHANGELOG.md Co-authored-by: Joris Vaillant --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7aa478bbdb..627e56f3d7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,7 +13,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Remove a lot of warnings ([#2139](https://github.com/stack-of-tasks/pinocchio/pull/2139)) ### Added -- Add example to visualize floating base velocity [#2143](https://github.com/stack-of-tasks/pinocchio/pull/2143) +- Add `examples/floating-base-velocity-viewer.py` to visualize floating base velocity [#2143](https://github.com/stack-of-tasks/pinocchio/pull/2143) ## [2.7.0] - 2024-01-23 From b8cf42f3a195b22b1488fb2d60fe87e074d65d0d Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Fri, 2 Feb 2024 10:59:49 +0100 Subject: [PATCH 4/7] Update examples/floating-base-velocity-viewer.py Co-authored-by: Joris Vaillant --- examples/floating-base-velocity-viewer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/floating-base-velocity-viewer.py b/examples/floating-base-velocity-viewer.py index a3bdfde390..c1c8da9510 100644 --- a/examples/floating-base-velocity-viewer.py +++ b/examples/floating-base-velocity-viewer.py @@ -18,7 +18,7 @@ def create_pin_cube_model(j0='freeflyer'): raise ValueError("Unknown joint type") jointCube = model.addJoint(0, j0, pin.SE3.Identity(), "joint0") - M = pin.SE3(np.eye(3), np.matrix([0.0, 0.0, 0.0]).T) + M = pin.SE3.Identity() model.appendBodyToJoint(jointCube, pin.Inertia.FromBox(1, 0.8, 0.4, 0.2), M) return model From 9ae7e2b2b7692ad33595ed586c09dd16558cc601 Mon Sep 17 00:00:00 2001 From: Fabian Schramm <55981657+fabinsch@users.noreply.github.com> Date: Fri, 2 Feb 2024 11:04:57 +0100 Subject: [PATCH 5/7] review: remove aba and just use v --- examples/floating-base-velocity-viewer.py | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/examples/floating-base-velocity-viewer.py b/examples/floating-base-velocity-viewer.py index c1c8da9510..fac49a5ecb 100644 --- a/examples/floating-base-velocity-viewer.py +++ b/examples/floating-base-velocity-viewer.py @@ -43,9 +43,6 @@ def pin_step(model, vizer, v_index_increment, dt = 0.1, enable_gravity=True): if not enable_gravity: model.gravity.setZero() - data = model.createData() - - tau = np.zeros(model.nv) q = pin.neutral(model) v = np.zeros(model.nv) v[v_index_increment] += 1. @@ -55,12 +52,10 @@ def pin_step(model, vizer, v_index_increment, dt = 0.1, enable_gravity=True): sleep(1) - a = pin.aba(model, data, q, v, tau) - v_next = v + a * dt - q_next = pin.integrate(model, q, v_next * dt) + q_next = pin.integrate(model, q, v * dt) - print(f"{a=}, {q_next=}, {v_next=}") - return q_next, v_next + print(f"{q_next=}") + return q_next if __name__ == "__main__": @@ -86,7 +81,7 @@ def pin_step(model, vizer, v_index_increment, dt = 0.1, enable_gravity=True): for i in range(model.nv): print(f"v[{i}] += 1") - q_next, v_next = pin_step(model, vizer, i, dt=0.2, enable_gravity=False) + q_next = pin_step(model, vizer, i, dt=0.2, enable_gravity=False) vizer.display(q_next) sleep(2) From 1c997ad5aac6d5d9f76a8342d340445e237ffddb Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 2 Feb 2024 11:52:40 +0100 Subject: [PATCH 6/7] examples: Remove enable_gravity and not used variables --- examples/floating-base-velocity-viewer.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/examples/floating-base-velocity-viewer.py b/examples/floating-base-velocity-viewer.py index fac49a5ecb..3ae882411b 100644 --- a/examples/floating-base-velocity-viewer.py +++ b/examples/floating-base-velocity-viewer.py @@ -38,11 +38,8 @@ def create_model(joint0_type='freeflyer'): geom_model = create_pin_geometry_cube_model(model) return model, geom_model -def pin_step(model, vizer, v_index_increment, dt = 0.1, enable_gravity=True): - - if not enable_gravity: - model.gravity.setZero() +def pin_step(model, vizer, v_index_increment, dt=0.1): q = pin.neutral(model) v = np.zeros(model.nv) v[v_index_increment] += 1. @@ -81,7 +78,6 @@ def pin_step(model, vizer, v_index_increment, dt = 0.1, enable_gravity=True): for i in range(model.nv): print(f"v[{i}] += 1") - q_next = pin_step(model, vizer, i, dt=0.2, enable_gravity=False) + q_next = pin_step(model, vizer, i, dt=0.2) vizer.display(q_next) sleep(2) - From 3d40f7c587055fd061bf2b8509f2149af1ffeb02 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 2 Feb 2024 11:53:22 +0100 Subject: [PATCH 7/7] examples: Apply black --- examples/floating-base-velocity-viewer.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/examples/floating-base-velocity-viewer.py b/examples/floating-base-velocity-viewer.py index 3ae882411b..f380abbf15 100644 --- a/examples/floating-base-velocity-viewer.py +++ b/examples/floating-base-velocity-viewer.py @@ -5,14 +5,15 @@ from pinocchio.visualize import MeshcatVisualizer from time import sleep -def create_pin_cube_model(j0='freeflyer'): + +def create_pin_cube_model(j0="freeflyer"): model = pin.Model() - if j0 == 'freeflyer': + if j0 == "freeflyer": j0 = pin.JointModelFreeFlyer() - elif j0 == 'spherical': + elif j0 == "spherical": j0 = pin.JointModelSpherical() - elif j0 == 'sphericalzyx': + elif j0 == "sphericalzyx": j0 = pin.JointModelSphericalZYX() else: raise ValueError("Unknown joint type") @@ -22,6 +23,7 @@ def create_pin_cube_model(j0='freeflyer'): model.appendBodyToJoint(jointCube, pin.Inertia.FromBox(1, 0.8, 0.4, 0.2), M) return model + def create_pin_geometry_cube_model(model): jointCube = model.getFrameId("joint0") geom_model = pin.GeometryModel() @@ -33,7 +35,8 @@ def create_pin_geometry_cube_model(model): geom_model.addGeometryObject(cube) return geom_model -def create_model(joint0_type='freeflyer'): + +def create_model(joint0_type="freeflyer"): model = create_pin_cube_model(joint0_type) geom_model = create_pin_geometry_cube_model(model) return model, geom_model @@ -42,7 +45,7 @@ def create_model(joint0_type='freeflyer'): def pin_step(model, vizer, v_index_increment, dt=0.1): q = pin.neutral(model) v = np.zeros(model.nv) - v[v_index_increment] += 1. + v[v_index_increment] += 1.0 vizer.display(q) print(f"{q=}, {v=}") @@ -56,11 +59,9 @@ def pin_step(model, vizer, v_index_increment, dt=0.1): if __name__ == "__main__": - - list_joint0 = ['freeflyer', 'sphericalzyx', 'spherical'] + list_joint0 = ["freeflyer", "sphericalzyx", "spherical"] for joint0_name in list_joint0: - print(f"\njoint0_name = {joint0_name}") model, geom_model = create_model(joint0_name)