Skip to content

Commit

Permalink
Working collision avoidance, yay.
Browse files Browse the repository at this point in the history
  • Loading branch information
@kevinzakka
kevinzakka committed Jul 7, 2024
1 parent c46b27a commit 1ddf42c
Show file tree
Hide file tree
Showing 89 changed files with 373 additions and 927,177 deletions.
8 changes: 8 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,3 +14,11 @@ pip install -e .
mjpython examples/arm_ur5e.py # On macOS
python examples/arm_ur5e.py # On Linux
```

## Background



## References

https://hal.science/hal-04621130/file/OpenSoT_journal_wip.pdf
44 changes: 21 additions & 23 deletions examples/arm_ur5e.py
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
import mujoco
import mujoco.viewer
import numpy as np
import mink
from pathlib import Path
from loop_rate_limiters import RateLimiter
Expand All @@ -25,45 +24,44 @@
),
]

# Enable collision avoidance between the following geoms:
# "wrist_3_link" and "floor"
# NOTE(kevin): Had to add "wrist_3_link" name to the geom.
collision_pairs = [
# (["wrist_3_link"], ["floor", "wall"]),
(["wrist_2_link_1", "wrist_2_link_2"], ["floor", "wall"]),
]

limits = [
mink.ConfigurationLimit(model=model),
mink.VelocityLimit(np.deg2rad(180) * np.ones_like(configuration.q)),
# mink.CollisionAvoidanceLimit(model=model, geom_pairs=collision_pairs),
]

mid = model.body("target").mocapid[0]
model = configuration.model
data = configuration.data
velocity = None
solver = "quadprog"

with mujoco.viewer.launch_passive(
model=model,
data=data,
show_left_ui=False,
show_right_ui=False,
model=model, data=data, show_left_ui=False, show_right_ui=False
) as viewer:
mujoco.mjv_defaultFreeCamera(model, viewer.cam)

# Initialize to the home keyframe.
mujoco.mj_resetDataKeyframe(model, data, model.key("home").id)
configuration.update()
configuration.update_from_keyframe("home")

# Initialize the mocap target at the end-effector site.
mocap_id = model.body("target").mocapid[0]
set_mocap_pose_from_site(model, data, "target", "attachment_site")

# Initialize the free camera.
mujoco.mjv_defaultFreeCamera(model, viewer.cam)

rate = RateLimiter(frequency=60.0)
dt = rate.period
sim_steps_per_control_steps = int(np.ceil(dt / model.opt.timestep))
rate = RateLimiter(frequency=500.0)
vel = None
while viewer.is_running():
# Update task target.
end_effector_task.set_target_from_mocap(data, mocap_id)
end_effector_task.set_target_from_mocap(data, mid)

# Compute velocity, integrate into position targets and set control signal.
velocity = mink.solve_ik(
configuration, tasks, limits, dt, prev_sol=velocity
)
data.ctrl = configuration.integrate(velocity, dt)
mujoco.mj_step(model, data, sim_steps_per_control_steps)
# Compute velocity and integrate into the next configuration.
vel = mink.solve_ik(configuration, tasks, limits, rate.dt, solver, 1e-3)
configuration.integrate_inplace(vel, rate.dt)

# Visualize at fixed FPS.
viewer.sync()
Expand Down
103 changes: 45 additions & 58 deletions examples/flying_dual_arm_ur5e.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,23 +32,21 @@ def construct_model():
base.pos = [-0.0 * width, -0.0 * height, -0.5 * depth]
base.add("freejoint")
base.add("site", name="base", pos=[0, 0, depth], group=1)
left_site = base.add(
"site", name="left_attachment_site", pos=[0.3, 0, depth], group=5
)
left_site = base.add("site", name="l_attachment_site", pos=[0.3, 0, depth], group=5)
right_site = base.add(
"site",
name="right_attachment_site",
name="r_attachment_site",
pos=[-0.3, 0, depth],
group=5,
)

left_ur5e = mjcf.from_path(_XML.as_posix())
left_ur5e.model = "left_ur5e"
left_ur5e.model = "l_ur5e"
left_ur5e.find("key", "home").remove()
left_site.attach(left_ur5e)

right_ur5e = mjcf.from_path(_XML.as_posix())
right_ur5e.model = "right_ur5e"
right_ur5e.model = "r_ur5e"
right_ur5e.find("key", "home").remove()
right_site.attach(right_ur5e)

Expand All @@ -62,7 +60,7 @@ def construct_model():
rgba=".6 .3 .3 .5",
)

body = root.worldbody.add("body", name="left_target", mocap=True)
body = root.worldbody.add("body", name="l_target", mocap=True)
body.add(
"geom",
type="box",
Expand All @@ -72,7 +70,7 @@ def construct_model():
rgba=".3 .6 .3 .5",
)

body = root.worldbody.add("body", name="right_target", mocap=True)
body = root.worldbody.add("body", name="r_target", mocap=True)
body.add(
"geom",
type="box",
Expand All @@ -87,79 +85,68 @@ def construct_model():

if __name__ == "__main__":
model = construct_model()
configuration = mink.Configuration(model)

base_task = mink.FrameTask(
frame_name="base",
frame_type="site",
position_cost=1.0,
orientation_cost=1.0,
)
left_ee_task = mink.FrameTask(
frame_name="left_ur5e/attachment_site",
frame_type="site",
position_cost=1.0,
orientation_cost=1.0,
)
right_ee_task = mink.FrameTask(
frame_name="right_ur5e/attachment_site",
frame_type="site",
position_cost=1.0,
orientation_cost=1.0,
)
configuration = mink.Configuration(model)

tasks = [base_task, left_ee_task, right_ee_task]
for task in tasks:
task.set_target_from_configuration(configuration)
tasks = [
base_task := mink.FrameTask(
frame_name="base",
frame_type="site",
position_cost=1.0,
orientation_cost=1.0,
),
left_ee_task := mink.FrameTask(
frame_name="l_ur5e/attachment_site",
frame_type="site",
position_cost=1.0,
orientation_cost=1.0,
),
right_ee_task := mink.FrameTask(
frame_name="r_ur5e/attachment_site",
frame_type="site",
position_cost=1.0,
orientation_cost=1.0,
),
]

limits = [
mink.ConfigurationLimit(model=model),
]

base_mid = model.body("base_target").mocapid[0]
left_mid = model.body("l_target").mocapid[0]
right_mid = model.body("r_target").mocapid[0]
model = configuration.model
data = configuration.data
velocity = None

with mujoco.viewer.launch_passive(
model=model,
data=data,
show_left_ui=False,
show_right_ui=False,
model=model, data=data, show_left_ui=False, show_right_ui=False
) as viewer:
mujoco.mjv_defaultFreeCamera(model, viewer.cam)
viewer.opt.frame = mujoco.mjtFrame.mjFRAME_SITE

base_mocap_id = model.body("base_target").mocapid[0]
set_mocap_pose_from_site(model, data, "base_target", "base")

left_mocap_id = model.body("left_target").mocapid[0]
set_mocap_pose_from_site(
model, data, "left_target", "left_ur5e/attachment_site"
)

right_mocap_id = model.body("right_target").mocapid[0]
set_mocap_pose_from_site(
model, data, "right_target", "right_ur5e/attachment_site"
)
set_mocap_pose_from_site(model, data, "l_target", "l_ur5e/attachment_site")
set_mocap_pose_from_site(model, data, "r_target", "r_ur5e/attachment_site")

rate = RateLimiter(frequency=200.0)
dt = rate.period
t = 0.0
vel = None
while viewer.is_running():
data.mocap_pos[base_mocap_id][2] = 0.3 * np.sin(2.0 * t)
base_task.set_target_from_mocap(data, base_mocap_id)
data.mocap_pos[base_mid][2] = 0.3 * np.sin(2.0 * t)
base_task.set_target_from_mocap(data, base_mid)

data.mocap_pos[left_mocap_id][1] = 0.5 + 0.2 * np.sin(2.0 * t)
data.mocap_pos[left_mocap_id][2] = 0.2
left_ee_task.set_target_from_mocap(data, left_mocap_id)
data.mocap_pos[left_mid][1] = 0.5 + 0.2 * np.sin(2.0 * t)
data.mocap_pos[left_mid][2] = 0.2
left_ee_task.set_target_from_mocap(data, left_mid)

data.mocap_pos[right_mocap_id][1] = 0.5 + 0.2 * np.sin(2.0 * t)
data.mocap_pos[right_mocap_id][2] = 0.2
right_ee_task.set_target_from_mocap(data, right_mocap_id)
data.mocap_pos[right_mid][1] = 0.5 + 0.2 * np.sin(2.0 * t)
data.mocap_pos[right_mid][2] = 0.2
right_ee_task.set_target_from_mocap(data, right_mid)

velocity = mink.solve_ik(configuration, tasks, limits, dt, 1e-2, velocity)
configuration.integrate_inplace(velocity, dt)
vel = mink.solve_ik(configuration, tasks, limits, rate.dt, 1e-2, vel)
configuration.integrate_inplace(vel, rate.dt)

viewer.sync()
rate.sleep()
t += dt
t += rate.dt
Loading

0 comments on commit 1ddf42c

Please sign in to comment.