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 test_forward_kinematics_on_inverted_pendulum():
mxml = MujocoXML.parse(
"test/inverted_pendulum/inverted_double_pendulum.xml").add_name_prefix(
"ivp:")
simulation = SimulationInterface(mxml.build())
simulation.register_joint_group("pendulum", "ivp:")
joint_names = list(map(lambda x: "ivp:%s" % x, ["hinge", "hinge2"]))
site_names = list(map(lambda x: "ivp:%s" % x, ["hinge2_site", "tip"]))
KIN = ForwardKinematics.prepare(mxml, "ivp:cart", np.zeros(3), np.zeros(3),
site_names, joint_names)
for _ in range(5):
simulation.mj_sim.data.ctrl[0] = random.random()
for _ in range(100):
simulation.step()
simulation.forward()
site_positions = np.array([
simulation.mj_sim.data.get_site_xpos(site) for site in site_names
])
joint_pos = simulation.get_qpos("pendulum")
kinemetics_positions = KIN.compute(joint_pos, return_joint_pos=True)
assert (np.abs(site_positions - kinemetics_positions[:2]) < 1e-6).all()
assert (np.abs(site_positions[0] - kinemetics_positions[-1]) <
1e-6).all()
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 __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_set_attributes_mixed_precision():
main = (MujocoXML().add_default_compiler_directive().append(
MujocoXML.from_string(XML_BALL).set_named_objects_attr(
"ball", pos=[1, 1e-8, 1e-12])))
simulation = SimulationInterface(main.build())
ball_id = simulation.sim.model.body_name2id("ball")
ball_pos = simulation.sim.model.body_pos[ball_id]
target_pos = np.array([1, 1e-8, 1e-12])
# test relative error cause absolute error can be quite small either way
assert np.linalg.norm((ball_pos / target_pos) - 1) < 1e-6
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
def __init__(self,
simulation: SimulationInterface,
robot_control_params: RobotControlParameters,
robot_prefix="robot0:",
autostep=False,
**kwargs):
"""
:param simulation: simulation interface for the mujoco shadow hand xml
:param hand_prefix: prefix to add to the joint names while constructing the mujoco simulation
:param autostep: when true, calls step() on the simulation whenever a control is set. this
should only be used only when the Robot is being controlled without a
simulationrunner in the loop.
"""
self.simulation = simulation
self.robot_prefix = robot_prefix
self.autostep = autostep
self.joint_group = robot_prefix + "gripper_joint_angles"
self.simulation.register_joint_group(
self.joint_group, prefix=[robot_prefix + "r_gripper_RJ0_outer"])
# assert max_position_change > 0.0, f"Position multiplier must be a positive number"
# For now, we do not constrain gripper motion.
self._max_position_change = None
self.actuator_id = self.mj_sim.model.actuator_name2id(
robot_prefix + "r_gripper_finger_joint")
# set a consistent initial control to prevent drifting at the start of the simulation
# otherwise the robot will try to go to the last control set, which may not be the same as the position)
# Since qpos is not set directly for gripper, simply set the ctrl to whatever position we have now
self.mj_sim.data.ctrl[self.actuator_id] = simulation.get_qpos(
self.joint_group).copy()
self.regrasp_helper = None
if robot_control_params.enable_gripper_regrasp:
self.regrasp_helper = RegraspHelper(
initial_position=self.observe().joint_positions())
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