From 40aa60facbdf6a4a0e4d49905b8855ce55270137 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?St=C3=A9phane=20Caron?= Date: Mon, 29 Jul 2024 14:23:23 +0200 Subject: [PATCH] Update example to velocity smoothing, detail how --- ...y_damping.py => arm_ur3_velocity_smoothing.py} | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) rename examples/{arm_ur3_velocity_damping.py => arm_ur3_velocity_smoothing.py} (91%) diff --git a/examples/arm_ur3_velocity_damping.py b/examples/arm_ur3_velocity_smoothing.py similarity index 91% rename from examples/arm_ur3_velocity_damping.py rename to examples/arm_ur3_velocity_smoothing.py index e40fbf8a..633bbdeb 100644 --- a/examples/arm_ur3_velocity_damping.py +++ b/examples/arm_ur3_velocity_smoothing.py @@ -47,7 +47,10 @@ ) acceleration_limit = AccelerationLimit( robot.model, - np.full(robot.model.nv, 1e4), + np.full( + robot.model.nv, + 20.0, # [rad] / [s]^2 + ), ) # Initial configuration and task setup @@ -91,20 +94,28 @@ ).np ) - # Compute velocity and integrate it into next configuration if step < nb_steps // 2: + # First half: no velocity smoothing + end_effector_task.gain = 1.0 tasks = (end_effector_task, posture_task) limits = ( configuration.model.configuration_limit, configuration.model.velocity_limit, ) else: # step >= nb_steps // 2 + # Second half: velocity smoothing by: + # 1. Reducing the task gain + # 2. Switching from a posture to a damping task + # 3. Adding an acceleration limit + end_effector_task.gain = 0.4 tasks = (end_effector_task, damping_task) limits = ( configuration.model.configuration_limit, configuration.model.velocity_limit, acceleration_limit, ) + + # Compute velocity and integrate it into next configuration velocity = solve_ik( configuration, tasks, dt, solver=solver, limits=limits )