def build_composite_robot(robot_control_params: RobotControlParameters,
simulation: SimulationInterface):
robot_cls: Optional[Callable[..., Robot]] = None
solver_simulation: Optional[SimulationInterface] = None
if robot_control_params.requires_solver_sim():
solver_simulation = build_solver_sim(
robot_control_params=robot_control_params,
n_substeps=simulation.n_substeps,
mujoco_timestep=simulation.mj_sim.model.opt.timestep,
)
robot_cls = (
MujocoURTcpJointGripperCompositeRobot # JointActuated, TCPControlled
)
elif robot_control_params.is_tcp_controlled():
solver_simulation = simulation
robot_cls = MujocoIdealURGripperCompositeRobot # MocapActuated, TCPControlled
elif robot_control_params.is_joint_actuated():
robot_cls = MujocoURJointGripperCompositeRobot # JointActuated, JointControlled
else:
raise ValueError(f"Unknown combination of robot_control_params to "
f"build a composite robot: {robot_control_params}")
assert robot_cls
robot = robot_cls(
simulation=simulation,
robot_control_params=robot_control_params,
solver_simulation=solver_simulation,
)
assert isinstance(robot, Robot)
return robot
def _build_reach_helper_test_robot(max_position_change=0.020):
from gym.envs.robotics import utils
from robogym.envs.rearrange.simulation.blocks import (
BlockRearrangeSim,
BlockRearrangeSimParameters,
)
from robogym.robot.robot_interface import ControlMode, RobotControlParameters
sim = BlockRearrangeSim.build(
n_substeps=20,
robot_control_params=RobotControlParameters(
control_mode=ControlMode.TCP_WRIST.value,
max_position_change=max_position_change,
),
simulation_params=BlockRearrangeSimParameters(),
)
# reset mocap welds if any. This is actually needed for TCP arms to move
utils.reset_mocap_welds(sim.mj_sim)
# extract arm since CompositeRobots are not fully supported by reach_helper
composite_robot = sim.robot
arm = composite_robot.robots[0]
arm.autostep = True
return arm
def __init__(
self,
simulation: ArmSimulationInterface,
robot_control_params: RobotControlParameters,
solver_simulation: ArmSimulationInterface,
robot_prefix="robot0:",
autostep=False,
controller_autostep=True,
):
"""
:param simulation: simulation interface for the mujoco UR arm
:param robot_control_params: Robot control parameters
:param robot_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.
:param controller_autostep: whether the controller arm should autostep.
For joint controlled TCP arm, this is set to true by default as we always want
the inner sim to be autostepping when a command is applied as it has a separate
solver_simulation that will not be stepped by the main env loop.
"""
assert robot_control_params.is_tcp_controlled()
super().__init__(
simulation=simulation,
robot_control_params=robot_control_params,
robot_prefix=robot_prefix,
autostep=autostep,
)
inner_tcp_solver_mode = robot_control_params.get_controller_arm_solver_mode(
)
inner_params = RobotControlParameters(
max_position_change=robot_control_params.max_position_change,
control_mode=robot_control_params.control_mode,
tcp_solver_mode=inner_tcp_solver_mode,
)
self.controller_arm: FreeDOFTcpArm = build_tcp_controller(
robot_control_params=inner_params,
initial_qpos=self.simulation.get_qpos(self.joint_group),
simulation=solver_simulation,
autostep=controller_autostep,
)
self.reset_controller_error = robot_control_params.arm_reset_controller_error
def test_free_wrist_reach(control_mode, tcp_solver_mode):
from robogym.envs.rearrange.blocks import make_env
max_position_change = RobotControlParameters.default_max_pos_change_for_solver(
control_mode=control_mode,
tcp_solver_mode=tcp_solver_mode,
arm_reset_controller_error=True,
)
env = make_env(
starting_seed=1,
parameters=dict(
robot_control_params=dict(
control_mode=control_mode,
tcp_solver_mode=tcp_solver_mode,
max_position_change=max_position_change,
arm_reset_controller_error=True,
),
n_random_initial_steps=10,
),
)
arm = env.robot.robots[0]
j6_lower_bound = arm.actuator_ctrl_range_lower_bound()[5]
j6_upper_bound = arm.actuator_ctrl_range_upper_bound()[5]
reach_buffer = np.deg2rad(5)
def _reset_and_get_arm(env):
env.reset()
arm = env.robot.robots[0]
arm.autostep = True
return arm
for target_angle in np.linspace(j6_lower_bound + reach_buffer,
j6_upper_bound - reach_buffer, 12):
arm = _reset_and_get_arm(env)
assert reach_wrist_angle(
robot=arm,
wrist_angle=target_angle,
max_steps=200,
Kp=2,
), (f"Failed reach. desired pos: {np.rad2deg(target_angle)} "
f"achieved pos: {np.rad2deg(arm.observe().joint_positions()[-1]):.2f}"
)
for unreachable_angle in [
j6_lower_bound - reach_buffer,
j6_upper_bound + reach_buffer,
]:
arm = _reset_and_get_arm(env)
assert not reach_wrist_angle(
robot=arm, wrist_angle=unreachable_angle
), f"Reached unreachable angle: {np.rad2deg(unreachable_angle)}"
def _build_arm(control_mode,
max_position_change,
tcp_solver_mode=TcpSolverMode.MOCAP):
sim = BlockRearrangeSim.build(
n_substeps=1,
simulation_params=BlockRearrangeSimParameters(),
robot_control_params=RobotControlParameters(
control_mode=control_mode,
max_position_change=max_position_change,
tcp_solver_mode=tcp_solver_mode,
),
)
return sim.robot.robots[0]
def _build_robot(control_mode, max_position_change):
sim = BlockRearrangeSim.build(
n_substeps=1,
robot_control_params=RobotControlParameters(
control_mode=control_mode,
max_position_change=max_position_change,
),
simulation_params=BlockRearrangeSimParameters(),
)
# reset mocap welds if any. This is actually needed for TCP arms to move, so other tests that did not use
# env may benefit from it.
utils.reset_mocap_welds(sim.mj_sim)
return sim.robot
def test_rearrange_defaults():
from robogym.robot.composite.ur_gripper_arm import (
MujocoURTcpJointGripperCompositeRobot, )
env = make_env()
assert isinstance(env.robot, MujocoURTcpJointGripperCompositeRobot)
assert (env.parameters.robot_control_params.max_position_change ==
RobotControlParameters.default_max_pos_change_for_solver(
control_mode=ControlMode.TCP_ROLL_YAW,
tcp_solver_mode=TcpSolverMode.MOCAP_IK,
))
assert env.parameters.robot_control_params.arm_reset_controller_error
assert env.parameters.robot_control_params.control_mode is ControlMode.TCP_ROLL_YAW
assert env.parameters.robot_control_params.tcp_solver_mode is TcpSolverMode.MOCAP_IK
assert env.action_space.shape == (6, )
def test_composite_autostep():
from robogym.robot.composite.ur_gripper_arm import (
MujocoURJointGripperCompositeRobot, )
sim = mock.MagicMock()
robot = MujocoURJointGripperCompositeRobot(
simulation=sim,
solver_simulation=sim,
robot_control_params=RobotControlParameters(max_position_change=0.05),
autostep=True,
)
assert sim.mj_sim.step.call_count == 0
robot.set_position_control(np.zeros(7))
assert sim.mj_sim.step.call_count == 1
def test_free_wrist_quat_constraint(
tcp_solver_mode: TcpSolverMode,
arm_joint_calibration_path: str,
off_axis_threshold: float,
):
"""
FreeWristTCP controlled env is only allowed to vary TCP quat about the y axis
this test checks for max deviation in the other axes when the arm is driven to
an extreme of the arm's reach so that mocap will deviate from the actual gripper
pos and may cause angle deviations if we have an alignment bug.
:return:
"""
from robogym.envs.rearrange.blocks import make_env
from robogym.utils import rotation
max_position_change = RobotControlParameters.default_max_pos_change_for_solver(
control_mode=ControlMode.TCP_WRIST,
tcp_solver_mode=tcp_solver_mode,
)
env = make_env(
starting_seed=3,
parameters=dict(robot_control_params=dict(
control_mode=ControlMode.TCP_WRIST,
tcp_solver_mode=tcp_solver_mode,
max_position_change=max_position_change,
arm_joint_calibration_path=arm_joint_calibration_path,
), ),
)
env.reset()
max_off_axis_deviation_deg = 0
for _ in range(50):
env.step([6, 10, 5, 5, 5])
angle = np.rad2deg(
rotation.quat2euler(
env.mujoco_simulation.mj_sim.data.get_body_xquat(
"robot0:gripper_base")))
x_rot = min(angle[0], 90 - angle[0])
z_rot = min(angle[2], 90 - angle[2])
max_off_axis_deviation_deg = np.max(
[max_off_axis_deviation_deg, x_rot, z_rot])
assert max_off_axis_deviation_deg < off_axis_threshold
def __init__(
self,
simulation: SimulationInterface,
robot_control_params: RobotControlParameters,
solver_simulation: ArmSimulationInterface,
robot_prefix="robot0:",
autostep=False,
):
"""
:param simulation: simulation interface for the mujoco UR arm
:param robot_control_params: Robot control parameters
:param robot_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.
"""
assert robot_control_params.is_tcp_controlled()
self.simulation = simulation
self.robot_prefix = robot_prefix
self.autostep = autostep
self.joint_group = robot_prefix + "arm_joint_angles"
self.simulation.register_joint_group(self.joint_group,
prefix=robot_prefix + "J")
self._max_position_change = robot_control_params.max_position_change
self.simulation.set_qpos(self.joint_group,
TABLETOP_EXPERIMENT_INITIAL_POS)
# Currently, we use the same simulation instance between this class and its controller
# arm, therefore, the controller_arm is never allowed to autostep to avoid double-stepping
# sim.
self.controller_arm: FreeDOFTcpArm = build_tcp_controller(
robot_control_params=robot_control_params,
initial_qpos=self.simulation.get_qpos(self.joint_group),
simulation=solver_simulation,
autostep=False,
)
self._is_in_joint_control_mode = False
def test_wrist_isolation(
tcp_solver_mode: TcpSolverMode,
control_mode: ControlMode,
off_joint_threshold_deg: float,
):
"""
FreeWristTCP controlled env is only allowed to vary TCP quat about the y axis
this test checks for max deviation in the other joints than J6 when only wrist
rotation action is applied.
:return:
"""
from robogym.envs.rearrange.blocks import make_env
max_position_change = RobotControlParameters.default_max_pos_change_for_solver(
control_mode=control_mode,
tcp_solver_mode=tcp_solver_mode,
)
env = make_env(
starting_seed=3,
parameters=dict(
n_random_initial_steps=0,
robot_control_params=dict(
control_mode=control_mode,
tcp_solver_mode=tcp_solver_mode,
max_position_change=max_position_change,
),
),
)
env.reset()
initial_joint_angles = env.observe()["robot_joint_pos"]
wrist_rot_action = np.ones(env.action_space.shape[0], dtype=np.int) * 5
wrist_joint_index = -2
# move wrist CW
wrist_rot_action[wrist_joint_index] = 7
for _ in range(100):
env.step(wrist_rot_action)
final_joint_angles = env.observe()["robot_joint_pos"]
assert np.allclose(
initial_joint_angles[:5],
final_joint_angles[:5],
atol=np.deg2rad(off_joint_threshold_deg),
)
# move wrist CCW
env.reset()
wrist_rot_action[wrist_joint_index] = 3
for _ in range(100):
env.step(wrist_rot_action)
final_joint_angles = env.observe()["robot_joint_pos"]
assert np.allclose(
initial_joint_angles[:5],
final_joint_angles[:5],
atol=np.deg2rad(off_joint_threshold_deg),
)
def test_table_collision_penalty(make_env, z_action, tcp_solver_mode):
"""
This test ensures table penalty is applied correctly when the gripper is in close proximity
of the table, and also tests it is not spuriously applied when it is not.
To achieve this, it applies a prescribed action in the Z direction to the arm and checks the
gripper penalties are calculated correctly.
:param make_env: make_env function to test
:param z_action: action to apply in the world Z direction. All other actions will be zero.
:param tcp_solver_mode: TCP solver mode to test.
"""
TABLE_COLLISION_PENALTY = 0.2
# Reward calculated by RobotEnv is of form [reward, goal_reward, success_reward].
ENV_REWARD_IDX = 0
SIM_REWARD_IDX = 1
max_position_change = RobotControlParameters.default_max_pos_change_for_solver(
control_mode=ControlMode.TCP_ROLL_YAW, tcp_solver_mode=tcp_solver_mode)
env = make_env(
parameters=dict(
n_random_initial_steps=0,
simulation_params=dict(
penalty=dict(table_collision=TABLE_COLLISION_PENALTY), ),
robot_control_params=dict(
tcp_solver_mode=tcp_solver_mode,
max_position_change=max_position_change,
),
),
constants=dict(use_goal_distance_reward=False),
starting_seed=0,
).env
env.reset()
# check condition at start
expect_initial_penalty = env.unwrapped.mujoco_simulation.get_gripper_table_contact(
)
if expect_initial_penalty:
assert (env.get_simulation_info_reward_with_done()[SIM_REWARD_IDX] ==
TABLE_COLLISION_PENALTY)
else:
assert env.get_simulation_info_reward_with_done(
)[SIM_REWARD_IDX] == 0.0
action = np.zeros_like(env.action_space.sample())
action[2] = z_action
for _ in range(20):
_, reward, _, _ = env.step(action)
expect_penalty = (
z_action < 0
) # expect penalty if the action is pushing the gripper towards the table
if expect_penalty:
# assert env.reward() == TABLE_COLLISION_PENALTY
assert (reward[ENV_REWARD_IDX] == -TABLE_COLLISION_PENALTY
) # goal reward should be negative
else:
# assert env.reward() == 0.0
assert reward[
ENV_REWARD_IDX] >= 0.0 # goal reward should be non-negative