-
Notifications
You must be signed in to change notification settings - Fork 12
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
16 changed files
with
1,079 additions
and
10 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,16 @@ | ||
:github_url: https://github.com/stephane-caron/pink/tree/main/doc/barriers.rst | ||
|
||
.. _Barriers: | ||
|
||
******** | ||
Barriers | ||
******** | ||
|
||
.. automodule:: pink.barriers | ||
:members: | ||
|
||
Position Barrier | ||
================= | ||
|
||
.. automodule:: pink.barriers.position_barrier | ||
:members: |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,48 @@ | ||
Here we will have the examples of using barriers, please go over [notes](NOTES.md) for more info. | ||
|
||
# Barrier Examples | ||
|
||
|
||
- [Arm: UR5](#arm-ur5): with joints and end effector limits | ||
- [Quadruped: Go2](#go2-squat): Go 2 squatting with floating base position limits | ||
- [Dual Arms: Yumi](#dual-arm-yumi): self collisions with spheres | ||
|
||
## Arm UR5 | ||
|
||
A UR5 arm tracking a moving target while stopping in front of virtual wall: | ||
|
||
|
||
https://github.com/domrachev03/pink/assets/28687492/f30ba7a1-98a3-44cb-ab52-23f99e42714c | ||
|
||
|
||
| Task | Cost | | ||
|------|------| | ||
| End-effector | (50,1) | | ||
| Posture | $10^{-3}$ | | ||
|
||
| Barrier | Gain | | ||
|------|------| | ||
| End-effector | $10^{2}$ | | ||
|
||
## Go2 squat | ||
|
||
Go2 quadruped squating with base position is constrained by z and y coordinates: | ||
|
||
https://github.com/domrachev03/pink/assets/26837717/701bdfbe-0dba-4f9d-80e2-c018475f38d6 | ||
|
||
|
||
|
||
| Task | Cost | | ||
|------|------| | ||
| Base | (50, 1) | | ||
| Legs | 200 | | ||
| Posture | $10^{-3}$ | | ||
|
||
| Barrier | Gain | | ||
|------|------| | ||
| End-effector | $10^{2}$ | | ||
|
||
|
||
|
||
|
||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,120 @@ | ||
#!/usr/bin/env python3 | ||
# -*- coding: utf-8 -*- | ||
# | ||
# SPDX-License-Identifier: Apache-2.0 | ||
# Copyright 2022 Stéphane Caron | ||
|
||
"""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.barriers import PositionBarrier | ||
from pink.tasks import FrameTask, PostureTask | ||
from pink.visualization import start_meshcat_visualizer | ||
|
||
try: | ||
from robot_descriptions.loaders.pinocchio import load_robot_description | ||
except ModuleNotFoundError as exc: | ||
raise ModuleNotFoundError( | ||
"Examples need robot_descriptions, " | ||
"try ``pip install robot_descriptions``" | ||
) from exc # noqa: E501 | ||
|
||
|
||
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=50.0, # [cost] / [m] | ||
orientation_cost=1.0, # [cost] / [rad] | ||
) | ||
|
||
posture_task = PostureTask( | ||
cost=1e-3, # [cost] / [rad] | ||
) | ||
|
||
pos_barrier = PositionBarrier( | ||
"ee_link", | ||
indices=[1], | ||
p_min=np.array([-0.4]), | ||
p_max=np.array([0.6]), | ||
gain=np.array([100.0]), | ||
r=1.0, | ||
) | ||
barriers_list = [pos_barrier] | ||
|
||
tasks = [end_effector_task, posture_task] | ||
|
||
q_ref = np.array( | ||
[ | ||
1.27153374, | ||
-0.87988708, | ||
1.89104795, | ||
1.73996951, | ||
-0.24610945, | ||
-0.74979019, | ||
] | ||
) | ||
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 "osqp" in qpsolvers.available_solvers: | ||
solver = "osqp" | ||
|
||
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.0 + 0.65 * np.sin(t / 4) | ||
end_effector_target.translation[2] = 0.5 | ||
|
||
# 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.frame | ||
).np | ||
) | ||
|
||
# Compute velocity and integrate it into next configuration | ||
# Note that default position limit handle given trajectory | ||
# much worse than CBF. Hence, we disable it here. | ||
velocity = solve_ik( | ||
configuration, | ||
tasks, | ||
dt, | ||
solver=solver, | ||
barriers=barriers_list, | ||
) | ||
configuration.integrate_inplace(velocity, dt) | ||
|
||
G, h = pos_barrier.compute_qp_inequality(configuration, dt=dt) | ||
print(f"Task error: {end_effector_task.compute_error(configuration)}") | ||
print( | ||
f"Position CBF value: {pos_barrier.compute_barrier(configuration)[0]:0.3f} >= 0" | ||
) | ||
print( | ||
f"Distance to manipulator: {configuration.get_transform_frame_to_world('ee_link').translation[1]} <= 0.6" | ||
) | ||
print("-" * 60) | ||
# Visualize result at fixed FPS | ||
viz.display(configuration.q) | ||
rate.sleep() | ||
t += dt |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,128 @@ | ||
#!/usr/bin/env python3 | ||
# -*- coding: utf-8 -*- | ||
# | ||
# SPDX-License-Identifier: Apache-2.0 | ||
# Copyright 2022 Stéphane Caron | ||
|
||
"""GO2 squat with z-axis barrier.""" | ||
|
||
import meshcat_shapes | ||
import numpy as np | ||
import qpsolvers | ||
from loop_rate_limiters import RateLimiter | ||
import pinocchio as pin | ||
from time import perf_counter | ||
import pink | ||
from pink import solve_ik | ||
from pink.barriers import PositionBarrier | ||
from pink.tasks import FrameTask, PostureTask | ||
from pink.visualization import start_meshcat_visualizer | ||
|
||
try: | ||
from robot_descriptions.loaders.pinocchio import load_robot_description | ||
except ModuleNotFoundError as exc: | ||
raise ModuleNotFoundError( | ||
"Examples need robot_descriptions, " | ||
"try ``pip install robot_descriptions``" | ||
) from exc # noqa: E501 | ||
|
||
|
||
if __name__ == "__main__": | ||
robot = load_robot_description( | ||
"go2_description", root_joint=pin.JointModelFreeFlyer()) | ||
viz = start_meshcat_visualizer(robot) | ||
|
||
q_ref = np.array([-0., 0., 0.3, 0., 0., 0.0, 1., | ||
0.0, 0.8, -1.57, | ||
0.0, 0.8, -1.57, | ||
0.0, 0.8, -1.57, | ||
0.0, 0.8, -1.57]) | ||
|
||
configuration = pink.Configuration( | ||
robot.model, robot.data, q_ref) | ||
|
||
base_task = FrameTask( | ||
"base", | ||
position_cost=50.0, # [cost] / [m] | ||
orientation_cost=1.0, # [cost] / [rad] | ||
) | ||
|
||
posture_task = PostureTask( | ||
cost=1e-5, # [cost] / [rad] | ||
) | ||
|
||
pos_barrier = PositionBarrier( | ||
"base", | ||
indices=[1,2], | ||
p_max=np.array([0, 0.35]), | ||
gain=np.array([100.0, 100.0]), | ||
r=1.0, | ||
) | ||
barriers_list = [pos_barrier] | ||
|
||
tasks = [base_task, posture_task] | ||
|
||
for foot in ["FL_foot", "FR_foot", "RL_foot", "RR_foot"]: | ||
task = FrameTask( | ||
foot, | ||
position_cost=200.0, # [cost] / [m] | ||
orientation_cost=0.0, # [cost] / [rad] | ||
) | ||
tasks.append(task) | ||
|
||
for task in tasks: | ||
task.set_target_from_configuration(configuration) | ||
|
||
|
||
viewer = viz.viewer | ||
opacity = 0.5 # Set the desired opacity level (0 transparent, 1 opaque) | ||
|
||
meshcat_shapes.frame(viewer["base_target"], opacity=1.0) | ||
meshcat_shapes.frame(viewer["base"], opacity=1.0) | ||
|
||
# Select QP solver | ||
solver = qpsolvers.available_solvers[0] | ||
if "osqp" in qpsolvers.available_solvers: | ||
solver = "osqp" | ||
|
||
rate = RateLimiter(frequency=200.0) | ||
dt = rate.period | ||
t = 0.0 # [s] | ||
period = 4 | ||
omega = 2 * np.pi / period | ||
while True: | ||
# Update task targets | ||
end_effector_target = base_task.transform_target_to_world | ||
phase = (t//period)%2 | ||
Ay = 0.1*(1-phase) | ||
Az = 0.2*phase | ||
|
||
end_effector_target.translation[1] = 0. + Ay * np.sin(omega*t) | ||
end_effector_target.translation[2] = 0.3 + Az * np.sin(omega*t) | ||
|
||
|
||
# Update visualization frames | ||
viewer["base_target"].set_transform(end_effector_target.np) | ||
viewer["base"].set_transform( | ||
configuration.get_transform_frame_to_world( | ||
base_task.frame | ||
).np | ||
) | ||
|
||
# Compute velocity and integrate it into next configuration | ||
# Note that default position limit handle given trajectory | ||
# much worse than CBF. Hence, we disable it here. | ||
|
||
velocity = solve_ik( | ||
configuration, | ||
tasks, | ||
dt, | ||
solver=solver, | ||
barriers=barriers_list, | ||
) | ||
configuration.integrate_inplace(velocity, dt) | ||
|
||
|
||
viz.display(configuration.q) | ||
rate.sleep() | ||
t += dt |
Oops, something went wrong.