Skip to content

minimal example: joint limit instability #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
slecleach wants to merge 2 commits into
base: main
Choose a base branch
from
Open

minimal example: joint limit instability #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5201a17
example implemented
slecleach Feb 6, 2024
fdd5ef0
example implemented
slecleach Feb 6, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 45 additions & 0 deletions examples/assets/rotor.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
<mujoco model="humanoid">

<statistic extent="2" center="0 0 1"/>

<option timestep="0.00555"/>

<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<geom type="capsule" condim="1" friction="1.0 0.05 0.05" solimp=".9 .99 .003" solref=".015 1" material="self"/>
<joint limited="true" type="hinge" damping="0.1" stiffness="5" armature=".007" solimplimit="0 .99 .01"/>
<site size=".04" group="3"/>
<default class="force-torque">
<site type="box" size=".01 .01 .02" rgba="1 0 0 1" />
</default>
<default class="touch">
<site type="capsule" rgba="0 0 1 .3"/>
</default>
</default>
</default>

<worldbody>
<geom name="floor" type="plane" conaffinity="1" size="100 100 .2" material="grid"/>

<body name="body" pos="1 1 1">
<inertial mass="10" pos="0 0 0" diaginertia="0.1 0.1 0.1"/>
<geom name="body" class="collision" type="box" size="0.42 0.11 0.08"/>

<body name="body_link" pos="0.292 0 0.188" gravcomp="0.9">
<!-- <joint name="link" class="link" type="hinge" axis="0 0 1" pos="0 0 0" range="-1000000 1000000" damping="10.0" armature=".01"/> -->
<joint name="link" class="link" type="hinge" axis="0 0 1" pos="0 0 0" range="-200 45" damping="10.0" armature=".01"/>
<!-- <joint name="link" class="link" type="hinge" axis="0 0 1" pos="0 0 0" range="-45 1000000" damping="10.0" armature=".01"/> -->
<!-- <joint name="link" class="link" type="hinge" axis="0 0 1" pos="0 0 0" range="-4 3" damping="10.0" armature=".01"/> -->
<!-- <joint name="link" class="link" type="hinge" axis="0 0 1" pos="0 0 0" range="-3 3" damping="10.0" armature=".01"/> -->
<!-- <joint name="link" class="link" type="hinge" axis="0 0 1" pos="0 0 0" range="-1 1" damping="10.0" armature=".01"/> -->
<inertial mass="1" diaginertia="0.1 0.1 0.1" pos="-0.008399 0.000272 -0.024603" />
<geom name="geom_0" class="collision" type="capsule" size="0.05 0.015" pos="0 0 -0.07"/>
<geom name="geom_1" class="collision" type="capsule" fromto="-0.015 -0.062 0 -0.015 0.062 0" size="0.05"/>
</body>
</body>
</worldbody>


</mujoco>

80 changes: 80 additions & 0 deletions examples/env/env_rotor.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
#%%
# Copyright (c) 2022 NVIDIA CORPORATION. All rights reserved.
# NVIDIA CORPORATION and its licensors retain all intellectual property
# and proprietary rights in and to this software, related documentation
# and any modifications thereto. Any use, reproduction, disclosure or
# distribution of this software and related documentation without an express
# license agreement from NVIDIA CORPORATION is strictly prohibited.

###########################################################################
# Rotor environment
###########################################################################

import os
import math
import warp as wp
import warp.sim
import matplotlib.pyplot as plt
import numpy as np

from environment import Environment, run_env, RenderMode, IntegratorType


class RotorEnvironment(Environment):
sim_name = "env_rotor"
env_offset = (2.0, 0.0, 2.0)
opengl_render_settings = dict(scaling=1.0)
usd_render_settings = dict(scaling=100.0)

sim_substeps_euler = 32
sim_substeps_xpbd = 5

xpbd_settings = dict(
iterations=2,
joint_linear_relaxation=0.7,
joint_angular_relaxation=0.5,
rigid_contact_relaxation=1.0,
rigid_contact_con_weighting=True,
)

def create_articulation(self, builder):
wp.sim.parse_mjcf(
os.path.join(os.path.dirname(__file__), "../assets/rotor.xml"),
builder,
stiffness=0.0,
damping=0.1,
armature=0.007,
armature_scale=10.0,
contact_ke=1.0e4,
contact_kd=1.0e2,
contact_kf=1.0e2,
contact_mu=0.5,
contact_restitution=0.0,
limit_ke=1.0e2,
limit_kd=1.0e1,
enable_self_collisions=True,
up_axis="y",
)

builder.joint_q[:] = [0.0]
builder.joint_qd[:] = [-2.0]

#%%
env = RotorEnvironment()
env.integrator_type = IntegratorType.XPBD
env.render_mode = RenderMode.NONE
env.num_envs = 1
env.profile = False
env.plot_joint_coords = True
env.init()

#%%
plot_joint_coords = env.run()
plt.plot(plot_joint_coords)
plt.xlabel("time_step")
plt.ylabel("angle in radian")
#%%
# if __name__ == "__main__":
# run_env(RotorEnvironment)

# %%
125 changes: 63 additions & 62 deletions examples/env/environment.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -425,68 +425,69 @@ def run(self):
ax[i, 6].yaxis.get_major_locator().set_params(integer=True)
plt.show()

if self.plot_joint_coords:
import matplotlib.pyplot as plt

joint_q_history = np.array(joint_q_history)
dof_q = joint_q_history.shape[1]
ncols = int(np.ceil(np.sqrt(dof_q)))
nrows = int(np.ceil(dof_q / float(ncols)))
fig, axes = plt.subplots(
ncols=ncols,
nrows=nrows,
constrained_layout=True,
figsize=(ncols * 3.5, nrows * 3.5),
squeeze=False,
sharex=True,
)

joint_id = 0
joint_type_names = {
wp.sim.JOINT_BALL: "ball",
wp.sim.JOINT_REVOLUTE: "hinge",
wp.sim.JOINT_PRISMATIC: "slide",
wp.sim.JOINT_UNIVERSAL: "universal",
wp.sim.JOINT_COMPOUND: "compound",
wp.sim.JOINT_FREE: "free",
wp.sim.JOINT_FIXED: "fixed",
wp.sim.JOINT_DISTANCE: "distance",
wp.sim.JOINT_D6: "D6",
}
joint_lower = self.model.joint_limit_lower.numpy()
joint_upper = self.model.joint_limit_upper.numpy()
joint_type = self.model.joint_type.numpy()
while joint_id < len(joint_type) - 1 and joint_type[joint_id] == wp.sim.JOINT_FIXED:
# skip fixed joints
joint_id += 1
q_start = self.model.joint_q_start.numpy()
qd_start = self.model.joint_qd_start.numpy()
qd_i = qd_start[joint_id]
for dim in range(ncols * nrows):
ax = axes[dim // ncols, dim % ncols]
if dim >= dof_q:
ax.axis("off")
continue
ax.grid()
ax.plot(joint_q_history[:, dim])
if joint_type[joint_id] != wp.sim.JOINT_FREE:
lower = joint_lower[qd_i]
if abs(lower) < 2 * np.pi:
ax.axhline(lower, color="red")
upper = joint_upper[qd_i]
if abs(upper) < 2 * np.pi:
ax.axhline(upper, color="red")
joint_name = joint_type_names[joint_type[joint_id]]
ax.set_title(f"$\\mathbf{{q_{{{dim}}}}}$ ({self.model.joint_name[joint_id]} / {joint_name} {joint_id})")
if joint_id < self.model.joint_count - 1 and q_start[joint_id + 1] == dim + 1:
joint_id += 1
qd_i = qd_start[joint_id]
else:
qd_i += 1
plt.tight_layout()
plt.show()

return 1000.0 * float(self.num_envs) / avg_time
# if self.plot_joint_coords:
# import matplotlib.pyplot as plt

# joint_q_history = np.array(joint_q_history)
# dof_q = joint_q_history.shape[1]
# ncols = int(np.ceil(np.sqrt(dof_q)))
# nrows = int(np.ceil(dof_q / float(ncols)))
# fig, axes = plt.subplots(
# ncols=ncols,
# nrows=nrows,
# constrained_layout=True,
# figsize=(ncols * 3.5, nrows * 3.5),
# squeeze=False,
# sharex=True,
# )

# joint_id = 0
# joint_type_names = {
# wp.sim.JOINT_BALL: "ball",
# wp.sim.JOINT_REVOLUTE: "hinge",
# wp.sim.JOINT_PRISMATIC: "slide",
# wp.sim.JOINT_UNIVERSAL: "universal",
# wp.sim.JOINT_COMPOUND: "compound",
# wp.sim.JOINT_FREE: "free",
# wp.sim.JOINT_FIXED: "fixed",
# wp.sim.JOINT_DISTANCE: "distance",
# wp.sim.JOINT_D6: "D6",
# }
# joint_lower = self.model.joint_limit_lower.numpy()
# joint_upper = self.model.joint_limit_upper.numpy()
# joint_type = self.model.joint_type.numpy()
# while joint_id < len(joint_type) - 1 and joint_type[joint_id] == wp.sim.JOINT_FIXED:
# # skip fixed joints
# joint_id += 1
# q_start = self.model.joint_q_start.numpy()
# qd_start = self.model.joint_qd_start.numpy()
# qd_i = qd_start[joint_id]
# for dim in range(ncols * nrows):
# ax = axes[dim // ncols, dim % ncols]
# if dim >= dof_q:
# ax.axis("off")
# continue
# ax.grid()
# ax.plot(joint_q_history[:, dim])
# if joint_type[joint_id] != wp.sim.JOINT_FREE:
# lower = joint_lower[qd_i]
# if abs(lower) < 2 * np.pi:
# ax.axhline(lower, color="red")
# upper = joint_upper[qd_i]
# if abs(upper) < 2 * np.pi:
# ax.axhline(upper, color="red")
# joint_name = joint_type_names[joint_type[joint_id]]
# ax.set_title(f"$\\mathbf{{q_{{{dim}}}}}$ ({self.model.joint_name[joint_id]} / {joint_name} {joint_id})")
# if joint_id < self.model.joint_count - 1 and q_start[joint_id + 1] == dim + 1:
# joint_id += 1
# qd_i = qd_start[joint_id]
# else:
# qd_i += 1
# plt.tight_layout()
# plt.show()

# return 1000.0 * float(self.num_envs) / avg_time
return joint_q_history


def run_env(Demo):
Expand Down