Add UR5 example

CHANGELOG.md

@@ -6,6 +6,7 @@ All notable changes to this project will be documented in this file.
 
 ### Added
 
+- Example: UR5 arm
 - Example: flying dual-arm with UR3
 
 ### Changed

examples/arm_ur5.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# Copyright 2022 Stéphane Caron
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Universal Robots UR5 arm tracking a moving target."""
+
+import meshcat_shapes
+import numpy as np
+import qpsolvers
+from loop_rate_limiters import RateLimiter
+
+import pink
+from pink import solve_ik
+from pink.tasks import FrameTask, PostureTask
+from pink.utils import custom_configuration_vector
+from pink.visualization import start_meshcat_visualizer
+
+try:
+    from robot_descriptions.loaders.pinocchio import load_robot_description
+except ModuleNotFoundError:
+    raise ModuleNotFoundError(
+        "Examples need robot_descriptions, "
+        "try ``pip install robot_descriptions``"
+    )
+
+
+if __name__ == "__main__":
+    robot = load_robot_description("ur5_description", root_joint=None)
+    viz = start_meshcat_visualizer(robot)
+
+    end_effector_task = FrameTask(
+        "ee_link",
+        position_cost=1.0,  # [cost] / [m]
+        orientation_cost=1.0,  # [cost] / [rad]
+        lm_damping=1.0,  # tuned for this setup
+    )
+
+    posture_task = PostureTask(
+        cost=1e-3,  # [cost] / [rad]
+    )
+
+    tasks = [end_effector_task, posture_task]
+
+    q_ref = custom_configuration_vector(
+        robot,
+        shoulder_lift_joint=1.0,
+        shoulder_pan_joint=1.0,
+        elbow_joint=1.0,
+    )
+    configuration = pink.Configuration(robot.model, robot.data, q_ref)
+    for task in tasks:
+        task.set_target_from_configuration(configuration)
+    viz.display(configuration.q)
+
+    viewer = viz.viewer
+    meshcat_shapes.frame(viewer["end_effector_target"], opacity=0.5)
+    meshcat_shapes.frame(viewer["end_effector"], opacity=1.0)
+
+    # Select QP solver
+    solver = qpsolvers.available_solvers[0]
+    if "quadprog" in qpsolvers.available_solvers:
+        solver = "quadprog"
+
+    rate = RateLimiter(frequency=200.0)
+    dt = rate.period
+    t = 0.0  # [s]
+    while True:
+        # Update task targets
+        end_effector_target = end_effector_task.transform_target_to_world
+        end_effector_target.translation[1] = 0.5 + 0.1 * np.sin(2.0 * t)
+        end_effector_target.translation[2] = 0.2
+
+        # Update visualization frames
+        viewer["end_effector_target"].set_transform(end_effector_target.np)
+        viewer["end_effector"].set_transform(
+            configuration.get_transform_frame_to_world(
+                end_effector_task.body
+            ).np
+        )
+
+        # Compute velocity and integrate it into next configuration
+        velocity = solve_ik(configuration, tasks, dt, solver=solver)
+        configuration.integrate_inplace(velocity, dt)
+
+        # Visualize result at fixed FPS
+        viz.display(configuration.q)
+        rate.sleep()
+        t += dt