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 = {}