def __init__(self, simulation: SimulationInterface, hand_prefix: str,
joint_group: str):
sim = simulation.mj_sim
self._joint_positions = simulation.get_qpos(joint_group).copy()
self._joint_controls = sim.data.ctrl[:6]
self._time = sim.data.time
self._joint_velocities = simulation.get_qvel(joint_group).copy()
self._tcp_xyz = sim.data.get_body_xpos(
f"{hand_prefix}{TCP_BODY_NAME}").copy()
self._tcp_vel = sim.data.get_body_xvelp(
f"{hand_prefix}{TCP_BODY_NAME}").copy()
quat = sim.data.get_body_xquat(f"{hand_prefix}{TCP_BODY_NAME}").copy()
self._tcp_rot = rotation.quat2euler(quat)
sensordata = sim.data.sensordata.copy()
force_sensor_id = simulation.mj_sim.model.sensor_name2id(
"toolhead_force")
sensor_adr = sim.model.sensor_adr[force_sensor_id]
sensor_dim = sim.model.sensor_dim[force_sensor_id]
self._tcp_force = sensordata[sensor_adr:sensor_adr + sensor_dim]
torque_sensor_id = simulation.mj_sim.model.sensor_name2id(
"toolhead_torque")
sensor_adr = sim.model.sensor_adr[torque_sensor_id]
sensor_dim = sim.model.sensor_dim[torque_sensor_id]
self._tcp_torque = sensordata[sensor_adr:sensor_adr + sensor_dim]
def __init__(
self,
simulation: SimulationInterface,
joint_group: str,
actuator_id: int,
):
self._time = simulation.mj_sim.data.time
sim = simulation.mj_sim
self._joint_positions = simulation.get_qpos(joint_group).copy()
self._joint_controls = sim.data.ctrl[actuator_id:actuator_id +
1].copy()
self._joint_vel = simulation.get_qvel(joint_group).copy()
def test_more_complex_mujoco_setup():
xml = (MujocoXML().add_default_compiler_directive().append(
MujocoXML.from_string(XML_ARM).add_name_prefix(
"arm_one:").set_named_objects_attr(
"arm_one:ball", pos=[0, 1, 0])).append(
MujocoXML.from_string(XML_ARM).add_name_prefix(
"arm_two:").set_named_objects_attr("arm_two:ball",
pos=[0, -1, 0])))
simulation = SimulationInterface(xml.build())
simulation.register_joint_group("arm_one", "arm_one:")
simulation.register_joint_group("arm_one_hinge", "arm_one:hinge_joint")
simulation.register_joint_group("arm_two", "arm_two:")
simulation.register_joint_group("arm_two_hinge", "arm_two:hinge_joint")
assert simulation.get_qpos("arm_one").shape == (2, )
assert simulation.get_qvel("arm_one").shape == (2, )
assert simulation.get_qpos("arm_two").shape == (2, )
assert simulation.get_qvel("arm_two").shape == (2, )
assert simulation.get_qpos("arm_one_hinge").shape == (1, )
assert simulation.get_qvel("arm_one_hinge").shape == (1, )
assert simulation.get_qpos("arm_two_hinge").shape == (1, )
assert simulation.get_qvel("arm_two_hinge").shape == (1, )
initial_qpos_one = simulation.get_qpos("arm_one")
initial_qpos_two = simulation.get_qpos("arm_two")
simulation.set_qpos("arm_one_hinge", 0.1)
# Chech that we are setting the right hinge joint
assert np.linalg.norm(simulation.get_qpos("arm_one") -
initial_qpos_one) > 0.09
assert np.linalg.norm(simulation.get_qpos("arm_two") -
initial_qpos_two) < 1e-6
def __init__(self, simulation: SimulationInterface, hand_prefix: str,
joint_group: str):
fingers = np.array([
simulation.mj_sim.data.get_site_xpos(hand_prefix + site)
for site in FINGERTIP_SITE_NAMES
])
reference = np.array([
simulation.mj_sim.data.get_site_xpos(hand_prefix + site)
for site in REFERENCE_SITE_NAMES
])
self._fingertip_positions = get_relative_positions(fingers, reference)
self._joint_positions = simulation.get_qpos(joint_group).copy()
self._joint_vel = simulation.get_qvel(joint_group).copy()
self._time = simulation.mj_sim.data.time
self._force_limits = simulation.mj_sim.model.actuator_forcerange.copy()
self._actuator_force = normalize_by_limits(
simulation.mj_sim.data.actuator_force, self._force_limits)
def test_simple_mujoco_setup():
ball_one = (MujocoXML.from_string(XML_BALL).add_name_prefix(
"ball_one:").set_named_objects_attr("ball_one:ball", pos=[1, 0, 0]))
ball_two = (MujocoXML.from_string(XML_BALL).add_name_prefix(
"ball_two:").set_named_objects_attr("ball_two:ball", pos=[-1, 0, 0]))
main = (MujocoXML().add_default_compiler_directive().append(
ball_one).append(ball_two))
simulation = SimulationInterface(main.build())
simulation.register_joint_group("ball_one", "ball_one:ball_joint")
simulation.register_joint_group("ball_two", "ball_two:ball_joint")
assert simulation.get_qpos("ball_one").shape == (7, )
assert simulation.get_qpos("ball_two").shape == (7, )
assert simulation.get_qvel("ball_one").shape == (6, )
assert simulation.get_qvel("ball_two").shape == (6, )
qpos1 = np.random.randn(3)
qrot1 = uniform_quat(global_randstate)
qpos1_combined = np.concatenate([qpos1, qrot1])
qpos2 = np.random.randn(3)
qrot2 = uniform_quat(global_randstate)
qpos2_combined = np.concatenate([qpos2, qrot2])
simulation.set_qpos("ball_one", qpos1_combined)
simulation.set_qpos("ball_two", qpos2_combined)
assert np.linalg.norm(simulation.get_qpos("ball_one") -
qpos1_combined) < 1e-6
assert np.linalg.norm(simulation.get_qpos("ball_two") -
qpos2_combined) < 1e-6