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 )