def test_Tx():
test_arm = TwoJoint()
# set up the Mujoco interface and config
robot_config = arm("twojoint")
interface = Mujoco(robot_config=robot_config, visualize=False)
interface.connect()
q_vals = np.linspace(0, 2 * np.pi, 50)
for q0 in q_vals:
for q1 in q_vals:
q = [q0, q1]
assert np.allclose(
robot_config.Tx("link0", q, object_type="geom"),
test_arm.Tx_link0(q),
atol=1e-5,
)
assert np.allclose(
robot_config.Tx("joint0", q, object_type="joint"),
test_arm.Tx_joint0(q),
atol=1e-5,
)
assert np.allclose(
robot_config.Tx("link1", q, object_type="geom"),
test_arm.Tx_link1(q),
atol=1e-5,
)
assert np.allclose(
robot_config.Tx("joint1", q, object_type="joint"),
test_arm.Tx_joint1(q))
assert np.allclose(robot_config.Tx("link2", q, object_type="geom"),
test_arm.Tx_link2(q))
assert np.allclose(robot_config.Tx("EE", q), test_arm.Tx_EE(q))
def test_g():
test_arm = TwoJoint()
# set up the Mujoco interface and config
robot_config = arm("twojoint")
interface = Mujoco(robot_config=robot_config, visualize=False)
interface.connect()
q_vals = np.linspace(0, 2 * np.pi, 50)
for q0 in q_vals:
for q1 in q_vals:
q = [q0, q1]
assert np.allclose(robot_config.g(q), test_arm.g(q))
def test_C():
test_arm = TwoJoint()
robot_config = arm("twojoint")
interface = Mujoco(robot_config=robot_config, visualize=False)
interface.connect()
with pytest.raises(NotImplementedError):
q_vals = np.linspace(0, 2 * np.pi, 15)
for q0 in q_vals:
for q1 in q_vals:
q = [q0, q1]
for dq0 in q_vals:
for dq1 in q_vals:
dq = [dq0, dq1]
assert np.allclose(robot_config.C(q, dq),
test_arm.C(q, dq))
def test_R():
test_arm = TwoJoint()
robot_config = arm("twojoint")
interface = Mujoco(robot_config=robot_config, visualize=False)
interface.connect()
q_vals = np.linspace(0, 2 * np.pi, 50)
for q0 in q_vals:
for q1 in q_vals:
q = [q0, q1]
assert np.allclose(robot_config.R("link1", q),
test_arm.R_link1(q),
atol=1e-5)
assert np.allclose(robot_config.R("link2", q),
test_arm.R_link2(q),
atol=1e-5)
assert np.allclose(robot_config.R("EE", q),
test_arm.R_EE(q),
atol=1e-5)
from abr_control.interfaces.mujoco import Mujoco
from abr_control.arms.mujoco_config import MujocoConfig
use_wall_clock = True
if len(sys.argv) > 1:
show_plot = sys.argv[1]
else:
show_plot = False
model_filename = 'threejoint'
robot_config = MujocoConfig(model_filename)
dt = 0.001
interface = Mujoco(robot_config, dt=dt)
interface.connect()
ctrlr = OSC(robot_config,
kp=10,
kv=5,
ctrlr_dof=[True, True, True, False, False, False])
interface.send_target_angles(np.ones(3))
target_xyz = np.array([0.1, 0.1, 0.3])
interface.set_mocap_xyz('target', target_xyz)
interface.set_mocap_xyz('hand', np.array([.2, .4, 1]))
# create our path planner
params = {}
