def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = np.array([
-0.498, 0.005, -0.431 + self.gripper_extra_height
]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
gripper_target = np.array([
-0.798, -0.25, -0.331 + self.gripper_extra_height
]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([0., 0., 0., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
'robot0:grip').copy()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
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 _env_setup(self, initial_qpos):
self.id2obj = [self._geom2objid(i) for i in range(self.sim.model.ngeom)]
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
if not hasattr(self, 'initial_gripper_xpos'):
self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
# Change the colors to match the success conditions
self.color_modder = TextureModder(self.sim)
for i in range(self.sim.model.ngeom):
obj_id = self.id2obj[i]
if obj_id != None:
name = self.sim.model.geom_id2name(i)
if 'table' in name:
color = np.asarray(COLORS_RGB[self.obj_colors[obj_id]])
self.color_modder.set_rgb(name, color * 2)
else:
self.color_modder.set_rgb(name,
COLORS_RGB[self.obj_colors[obj_id]])
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
if self.cam_type != "fixed":
delta_pos = self.np_random.uniform(-0.03, 0.03, size=3)
self.cam_offset = delta_pos
# delta_rot = self.np_random.uniform(-0.1, 0.1, size=3)
delta_rot = np.array([0, 0, 0])
utils.cam_init_pos(self.sim, delta_pos, delta_rot)
self.sim.forward()
# Move end effector into position.
if self.gripper_init_type != "fixed":
init_disturbance = np.array([
self.np_random.uniform(-0.05, 0.05),
self.np_random.uniform(-0.05, 0.05), 0.2
])
else:
init_disturbance = np.array([0, 0, 0.2])
gripper_target = np.array([
-0.498, 0.005, -0.431 + self.gripper_extra_height
]) + init_disturbance + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
'robot0:grip').copy()
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos, initial_rot):
'''
Render goals here acording to the basic motion
'''
print('************* Initial env setup *************')
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip')
print('Set initial position of the endefector = ', gripper_target)
gripper_rotation = initial_rot
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
self._sample_goals()
assert len(self.goals) >0
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
def _env_setup(self, initial_qpos):
"""
TODO: We can initialize the end effector using this method.
"""
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
gym_robotics_utils.reset_mocap_welds(self.sim)
self.sim.forward()
if self.easy_init:
### new position, closer to hook at start ###
gripper_target = np.array([-0.7, -0.4, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip')
else:
### the old, initial position ###
gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# compute Middle point
# TODO: initial markers (index 3 nur zufällig, aufpassen!)
self.middle_point = self.sim.data.get_site_xpos('middle_point')
self.middle_point[2] += 0.2 # adapt height
sites_offset = (self.sim.data.site_xpos - self.sim.model.site_pos).copy()[3]
# Move end effector into position. # TODO: changed that to the left
#gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_target = self.middle_point + self.gripper_extra_height #+ self.sim.data.get_site_xpos('robot0:grip')
gripper_target[0] -= self.target_range_x
gripper_target[1] += self.target_range_y
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
object_xpos = self.initial_gripper_xpos
object_xpos[2] = 0.4 # table height
site_id = self.sim.model.site_name2id('init_1')
self.sim.model.site_pos[site_id] = object_xpos + [self.obj_range, self.obj_range, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('init_2')
self.sim.model.site_pos[site_id] = object_xpos + [self.obj_range, -self.obj_range, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('init_3')
self.sim.model.site_pos[site_id] = object_xpos + [-self.obj_range, self.obj_range, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('init_4')
self.sim.model.site_pos[site_id] = object_xpos + [-self.obj_range, -self.obj_range, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark1')
self.sim.model.site_pos[site_id] = self.middle_point + [-2*self.target_range_x, 0.0, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark2')
self.sim.model.site_pos[site_id] = self.middle_point + [2*self.target_range_x, 0.0, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark3')
self.sim.model.site_pos[site_id] = self.middle_point + [0.0, 2*self.target_range_y, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark4')
self.sim.model.site_pos[site_id] = self.middle_point + [0.0, -2*self.target_range_y, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark5')
self.sim.model.site_pos[site_id] = self.middle_point + [2*self.target_range_x, 2*self.target_range_y, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark6')
self.sim.model.site_pos[site_id] = self.middle_point + [-2*self.target_range_x, 2*self.target_range_y, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark7')
self.sim.model.site_pos[site_id] = self.middle_point + [2*self.target_range_x, -2*self.target_range_y, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark8')
self.sim.model.site_pos[site_id] = self.middle_point + [-2*self.target_range_x, -2*self.target_range_y, 0.0] - sites_offset
self.sim.step()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# initial markers (index 3 is arbitrary)
self.target_1 = self.sim.data.get_site_xpos('target_1')
self.target_2 = self.sim.data.get_site_xpos('target_2')
self.target_3 = self.sim.data.get_site_xpos('target_3')
self.target_4 = self.sim.data.get_site_xpos('target_4')
self.target_5 = self.sim.data.get_site_xpos('target_5')
self.target_6 = self.sim.data.get_site_xpos('target_6')
self.target_7 = self.sim.data.get_site_xpos('target_7')
self.target_8 = self.sim.data.get_site_xpos('target_8')
self.targets = [self.target_1, self.target_2, self.target_3, self.target_4, self.target_5, self.target_6, self.target_7, self.target_8]
self.init_center = self.sim.data.get_site_xpos('init_center')
sites_offset = (self.sim.data.site_xpos - self.sim.model.site_pos).copy()[3]
# Move end effector into position.
gripper_target = self.init_center + self.gripper_extra_height #+ self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
object_xpos = self.initial_gripper_xpos
object_xpos[2] = 0.4 # table height
site_id = self.sim.model.site_name2id('init_1')
self.sim.model.site_pos[site_id] = object_xpos + [self.obj_range, self.obj_range, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('init_2')
self.sim.model.site_pos[site_id] = object_xpos + [self.obj_range, -self.obj_range, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('init_3')
self.sim.model.site_pos[site_id] = object_xpos + [-self.obj_range, self.obj_range, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('init_4')
self.sim.model.site_pos[site_id] = object_xpos + [-self.obj_range, -self.obj_range, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark1')
self.sim.model.site_pos[site_id] = self.target_1 + [self.target_range_x, self.target_range_y, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark2')
self.sim.model.site_pos[site_id] = self.target_1 + [-self.target_range_x, self.target_range_y, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark3')
self.sim.model.site_pos[site_id] = self.target_1 + [self.target_range_x, -self.target_range_y, 0.0] - sites_offset
site_id = self.sim.model.site_name2id('mark4')
self.sim.model.site_pos[site_id] = self.target_1 + [-self.target_range_x, -self.target_range_y, 0.0] - sites_offset
self.sim.step()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
'robot0:grip').copy()
def dobot_env_setup(sim):
initial_qpos = {
'robot0:slide0': 0.0,
'robot0:slide1': 0.0,
'robot0:slide2': 0.0,
}
for name, value in initial_qpos.items():
sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(sim)
sim.forward()
for _ in range(10):
sim.step()
def reset(self):
for name, value in self.initial_qpos.items():
self.data.set_joint_qpos(name, value)
reset_mocap_welds(self.sim)
self.sim.forward()
# not sure if this is required. therefore not using currently
# self.sim.reset()
ob = self.reset_model()
if self.viewer is not None:
self.viewer_setup()
return ob
def _env_setup(self, initial_qpos):
if initial_qpos is not None:
raise NotImplementedError
reset_mocap_welds(self.sim)
self.sim.forward()
self.init_qpos = self.sim.data.qpos.ravel().copy()
self.init_qvel = self.sim.data.qvel.ravel().copy()
yumi_arm_joints = [1, 2, 7, 3, 4, 5, 6]
if self.has_right_arm:
self._arm_r_joint_idx = [
self.sim.model.joint_name2id(f'yumi_joint_{i}_r')
for i in yumi_arm_joints
]
self.arm_r_joint_lims = self.sim.model.jnt_range[
self._arm_r_joint_idx].copy()
if self.has_left_arm:
self._arm_l_joint_idx = [
self.sim.model.joint_name2id(f'yumi_joint_{i}_l')
for i in yumi_arm_joints
]
self.arm_l_joint_lims = self.sim.model.jnt_range[
self._arm_l_joint_idx].copy()
if not self.block_gripper:
if self.has_right_arm:
self._gripper_r_joint_idx = [
self.sim.model.joint_name2id('gripper_r_joint'),
self.sim.model.joint_name2id('gripper_r_joint_m')
]
if self.has_left_arm:
self._gripper_l_joint_idx = [
self.sim.model.joint_name2id('gripper_l_joint'),
self.sim.model.joint_name2id('gripper_l_joint_m')
]
# Extract information for sampling goals.
if self.has_left_arm:
self._initial_l_gripper_pos = self.sim.data.get_site_xpos(
'gripper_l_center').copy()
if self.has_right_arm:
self._initial_r_gripper_pos = self.sim.data.get_site_xpos(
'gripper_r_center').copy()
if self.has_object:
for _ in range(10):
self.sim.step()
self._object_z_offset = self.sim.data.get_body_xpos('object0')[2]
self._reset_sim()
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
# gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('r_grip_site')
gripper_target = np.array([
0.498, 0.005, 0.431 + self.gripper_extra_height
]) + self.sim.data.get_site_xpos('r_grip_site')
if self.debug_print:
print("gripper quat: ", self.sim.data.get_site_xmat('r_grip_site'))
print("get_mocap_quat: ",
self.sim.data.get_mocap_quat('gripper_r:mocap'))
# gripper_rotation = np.array([1., 0., 1., 0.])
# gripper_rotation = np.array([-0.82031777, -0.33347336, -0.32553968, 0.33150896])
# gripper_target = np.array([0.9, -0.3, 0.6])
gripper_target = np.array([0.5, -0.3, 0.6])
# gripper_rotation = np.array([0.5, -0.5, -0.5, -0.5]) #(-90, 0, -90)
# gripper_rotation = np.array([0.5, 0.5, 0.5, -0.5]) #(90, 0, 90) gripper down
# gripper_rotation = np.array([0.707, 0.0, 0.0, -0.707]) #(0, 0, -90)
gripper_rotation = np.array([0, 0.707, 0.707, 0]) #(0, 0, -90)
# gripper_rotation = np.array([1.0, 0, 0, 0])
# set random gripper position
# for i in range(3):
# gripper_target[i] += self.np_random.uniform(-0.2, 0.2)
# print("gripper target random: ", gripper_target)
self.sim.data.set_mocap_pos('gripper_r:mocap', gripper_target)
self.sim.data.set_mocap_quat('gripper_r:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# # set random end-effector position
# end_effector_pos = self.initial_gripper_xpos
# rot_ctrl = [0, 0.707, 0.707, 0] #(0 0 0)
# end_effector_pos = np.concatenate([end_effector_pos, rot_ctrl])
# print("end_effector_pos: ", end_effector_pos)
# for i in range(3):
# end_effector_pos[i] = self.initial_gripper_xpos[i] + self.np_random.uniform(-0.1, 0.1)
# utils.mocap_set_action(self.sim, end_effector_pos) # arm control in cartesion (x, y, z)
# # for _ in range(100):
# # self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
'r_grip_site').copy()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
self._set_arm_pose(self._initial_arm_mocap_pose.copy())
for _ in range(10):
self.sim.step()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos(
'object:center')[2]
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
self.initial_pusher_xpos = self.sim.data.get_site_xpos('pusher').copy()
pusher_target = self.initial_pusher_xpos[:3]
pusher_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('ur5_mocap', pusher_target)
self.sim.data.set_mocap_quat('ur5_mocap', pusher_rotation)
for _ in range(10):
self.sim.step()
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 _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
self._set_gripper_during_setup()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
'robot0:grip').copy()
if self.has_object:
#self.height_offset = self.sim.data.get_site_xpos('object0')[2]
#todo
self.height_offset = 0.425
def _initialize_sim_state(self):
if self.parameters.robot_control_params.is_joint_actuated():
for idx, eq_type in enumerate(self.mujoco_simulation.mj_sim.model.eq_type):
if eq_type == MujocoEquality.mjEQ_WELD.value:
self.mujoco_simulation.mj_sim.model.eq_active[idx] = 0
else:
utils.reset_mocap_welds(self.sim)
self.sim.forward()
tcp_pos = self.mujoco_simulation.mj_sim.data.get_body_xpos(
"robot0:gripper_base"
)
tcp_quat = self.sim.data.get_body_xquat("robot0:gripper_base")
self.mujoco_simulation.mj_sim.data.set_mocap_pos("robot0:mocap", tcp_pos)
self.mujoco_simulation.mj_sim.data.set_mocap_quat("robot0:mocap", tcp_quat)
for _ in range(10):
self.mujoco_simulation.step()
self.robot.reset()
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = np.array([0.6, 0.6 , 0.4 + self.gripper_extra_height]) #+ self.sim.data.get_site_xpos('robotiq_85_base_link')
gripper_rotation = np.array([0., 1., 1., 0.])
self.sim.data.set_mocap_pos('robot1:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot1:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_body_xpos('robot1:ee_link').copy() # Needs a change if using the gripper for goal generation
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
robot_utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = np.array([
-0.498, 0.005, -0.431 + self.gripper_extra_height
]) + self.sim.data.get_site_xpos("robot0:grip")
gripper_rotation = np.array([1.0, 0.0, 1.0, 0.0])
self.sim.data.set_mocap_pos("robot0:mocap", gripper_target)
self.sim.data.set_mocap_quat("robot0:mocap", gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
"robot0:grip").copy()
def _env_setup(self, initial_qpos):
# TODO: initial state of joints
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = self.sim.data.get_body_xpos('robot:left_outer_finger').copy() #+ np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height])
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_body_xpos('robot:left_outer_finger').copy()
def _env_setup(self):
utils.reset_mocap_welds(self.sim)
utils.reset_mocap2body_xpos(self.sim)
lim1_id = self.sim.model.site_name2id('limit0')
lim2_id = self.sim.model.site_name2id('limit1')
lim3_id = self.sim.model.site_name2id('limit2')
lim4_id = self.sim.model.site_name2id('limit3')
self.sim.model.site_pos[lim1_id] = self.origin + \
np.array([-self.maxdist, -self.maxdist, 0])
self.sim.model.site_pos[lim2_id] = self.origin + \
np.array([self.maxdist, -self.maxdist, 0])
self.sim.model.site_pos[lim3_id] = self.origin + \
np.array([-self.maxdist, self.maxdist, 0])
self.sim.model.site_pos[lim4_id] = self.origin + \
np.array([self.maxdist, self.maxdist, 0])
for _ in range(10):
self._take_substeps()
def _env_setup(self, initial_qpos):
if self.mode == "robot":
#self.reset_publisher.publish(String("RESET"))
#self.get_observation_command_publisher.publish(String("GET_OBS"))
#self.current_observation = np.array(rospy.wait_for_message(
# "/pyrobot/observation", numpy_msg(Floats)).data)
#eff_pos = self.current_observation[:3]
#self.initial_gripper_xpos = eff_pos
# DEBUG:
#rospy.loginfo('[DEBUG] env setup initial gripper xpos = {}'.format(self.initial_gripper_xpos))
pass
elif self.mode == "sim":
# Setup the initial joint positions
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
self.initial_state_2 = self.sim.get_state()
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into positions | -0.498, 0.005, -0.431
# + self.gripper_extra_height
# DEBUG:
#print('[ENV SETUP][BEFORE] initial_gripper_xpos = {}'.format(self.sim.data.get_site_xpos('robot0:end_effector')))
#gripper_target = np.array([-0.498, 0.005, -0.431+ self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:end_effector')
#gripper_target = np.array([-0.298, 0.005, -0.431])+ self.sim.data.get_site_xpos('robot0:end_effector')
#gripper_target = self.sim.data.get_site_xpos('robot0:end_effector')
#gripper_target = np.array([1.15360479, 0.74419554, 0.40428726])
#gripper_target = np.array([1.10412612, 0.744193, 0.17162448])
#gripper_rotation = np.array([1., 0., 1., 0.])
#gripper_rotation = np.array([0., 0., 0., 1.])
#print('[BEFORE] sim.data.mocap_pos = {}'.format(self.sim.data.mocap_pos))
#print('gripper_target = {}'.format(gripper_target))
#self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
#self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
#print('[After] sim.data.mocap_pos = {}'.format(self.sim.data.mocap_pos))
#self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:end_effector').copy()
#print('[ENV SETUP][AFTER] initial_gripper_xpos = {}'.format(self.initial_gripper_xpos))
#for _ in range(10):
# self.sim.step()
self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:end_effector').copy()
print('[ENV SETUP][AFTER] initial_gripper_xpos = {}'.format(self.initial_gripper_xpos))
print('[ENV SETUP][AFTER] sim.data.mocap_pos = {}'.format(self.sim.data.mocap_pos))
def _env_setup(self, initial_qpos):
for name in initial_qpos:
self.sim.data.set_joint_qpos(name, initial_qpos[name])
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move gripper into position
gripper_target = np.array(
[0.0, 0.0, 0.0]) + self.sim.data.get_site_xpos('robot0:gripper')
gripper_rotation = np.array([1., 0., 0., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
'robot0:gripper').copy()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _reset_sim(self):
RobotEnv._reset_sim(self)
# Move end-effector into position
gripper_rotation = np.array([1., 0., 1., 0.])
if self.random_initial_gripper_position:
# Limits of actuation: right on the back edge to just before the front edge
x = np.random.uniform(1.05, 1.5)
# A little beyond the left edge to a little beyond the right edge
y = np.random.uniform(0.3, 1.2)
# Right on the table to medium-high
z = np.random.uniform(0.45, 0.8)
gripper_target = np.array([x, y, z])
else:
# Above middle of table
gripper_target = [1.3419, 0.7491, 0.755]
self.sim.data.set_mocap_pos('mocap', gripper_target)
self.sim.data.set_mocap_quat('mocap', gripper_rotation)
self.sim.model.eq_active[0] = 1 # Enable mocap
# Not sure what this does, but FetchEnv has it
utils.reset_mocap_welds(self.sim)
self.sim.forward()
for _ in range(10):
self.sim.step()
# The actuators will try to fight against the mocap control.
# From https://github.com/openai/gym/issues/234, it seems like we can't
# turn off the actuators (by adjusting e.g. actuator_gainprm).
# So instead, we just update the actuators to be closer to where the
# mocap is trying to pull them.
for n, actuator_name in enumerate(self.sim.model.actuator_names):
pos = self.sim.data.get_joint_qpos(actuator_name)
self.sim.data.ctrl[n] = pos
# Disable mocap and give gripper time to stop bouncing
self.sim.model.eq_active[0] = 0
for _ in range(10):
self.sim.step()
return True
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position. Fetch:-0.498, 0.005, -0.431 baxter:-0.151, -0.831, -0.602
# gripper_target = np.array([0.151, 0.431, 0 + self.gripper_extra_height]) + self.sim.data.get_site_xpos('robot0:grip') #---baxter
# gripper_target = np.array([0.5824, 0.1861, 0.1207]) #---baxter
# gripper_rotation = np.array([1., 0., 1., 0.]) #---baxter [0 0 1 0]
# self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
# self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
# for _ in range(10): # ------------baxter
# self.sim.step() # ------------baxter
# self.sim.step() # ------------baxter
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
'robot0:grip').copy()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
# qpos = self.sim.data.get_joint_qpos(name)
# print(f"{name}: {qpos}")
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height]) \
+ self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()
self.initial_heights = {}
for obj_key in self.obj_keys:
self.initial_heights[obj_key] = self.sim.data.get_site_xpos(obj_key)[2]
def _env_setup(self, initial_qpos):
# TODO: initial state of joints
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = self.sim.data.get_body_xpos(self.finder_name).copy(
) #+ np.array([-0.498, 0.005, -0.431 + self.gripper_extra_height])
# gripper_rotation is 1 0 1 0 for wirst_1_link as finder_name
gripper_rotation = np.array([0., 0., 1., 0.])
self.sim.data.set_mocap_pos(self.mocap_name, gripper_target)
self.sim.data.set_mocap_quat(self.mocap_name, gripper_rotation)
# move gripper into position
for i in range(3):
self.sim.data.ctrl[i] = 0.8
for _ in range(10):
self.sim.step()
# move object into position
ref_pos = self.sim.data.get_body_xpos(self.finder_name)
offset = np.array([0., 0., -0.12])
pos = ref_pos + offset
rot = [0, 0, 0, 0]
ob_pos = list(pos) + rot
self.sim.data.set_joint_qpos("object0:joint", ob_pos)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_object_xpos = self.sim.data.get_body_xpos(
self.object_name).copy()
self.initial_gripper_xpos = self.sim.data.get_body_xpos(
self.finder_name).copy()
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
self.sim.data.set_mocap_quat("robot0:mocap",
np.array([1.0, 0.0, 1.0, 0.0]))
gripper_target = np.array([
-0.59, 0.0, -0.3 + self.gripper_extra_height
]) + self.sim.data.get_site_xpos("robot0:grip")
self._env_setup_helper(gripper_target, 1.0)
gripper_target = np.array(
[0.0, 0.0, -0.07]) + self.sim.data.get_site_xpos("robot0:grip")
self._env_setup_helper(gripper_target, 1.0)
gripper_target = np.array(
[0.0, 0.0, 0.0]) + self.sim.data.get_site_xpos("robot0:grip")
self._env_setup_helper(gripper_target, -1.0)
gripper_target = np.array(
[0.0, 0.0, -0.05]) + self.sim.data.get_site_xpos("robot0:grip")
self._env_setup_helper(gripper_target, -1.0)
# Extract information for sampling goals.
self.goal = self.sim.data.get_site_xpos("hole").copy() + np.array(
(0.0, 0.0, 0.0))
def _env_setup(self, initial_qpos):
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
utils.reset_mocap_welds(self.sim)
self.sim.forward()
# Move end effector into position.
gripper_target = np.array([
-0.498, 0.005, -0.431 + self.gripper_extra_height
]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
for _ in range(10):
self.sim.step()
# Extract information for sampling goals.
self.initial_gripper_xpos = self.sim.data.get_site_xpos(
'robot0:grip').copy()
# TODO: initial markers (index 2 nur zufällig, aufpassen!)
object_xpos = self.initial_gripper_xpos
object_xpos[2] = 0.4
sites_offset = (self.sim.data.site_xpos -
self.sim.model.site_pos).copy()[2]
site_id = self.sim.model.site_name2id('init_1')
self.sim.model.site_pos[site_id] = object_xpos + [
self.obj_range, self.obj_range, 0.0
] - sites_offset
site_id = self.sim.model.site_name2id('init_2')
self.sim.model.site_pos[site_id] = object_xpos + [
self.obj_range, -self.obj_range, 0.0
] - sites_offset
site_id = self.sim.model.site_name2id('init_3')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.obj_range, self.obj_range, 0.0
] - sites_offset
site_id = self.sim.model.site_name2id('init_4')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.obj_range, -self.obj_range, 0.0
] - sites_offset
site_id = self.sim.model.site_name2id('init_1a')
self.sim.model.site_pos[site_id] = object_xpos + [
self.obj_range, self.obj_range, self.air_height / 2
] - sites_offset
site_id = self.sim.model.site_name2id('init_2a')
self.sim.model.site_pos[site_id] = object_xpos + [
self.obj_range, -self.obj_range, self.air_height / 2
] - sites_offset
site_id = self.sim.model.site_name2id('init_3a')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.obj_range, self.obj_range, self.air_height / 2
] - sites_offset
site_id = self.sim.model.site_name2id('init_4a')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.obj_range, -self.obj_range, self.air_height / 2
] - sites_offset
site_id = self.sim.model.site_name2id('init_1b')
self.sim.model.site_pos[site_id] = object_xpos + [
self.obj_range, self.obj_range, self.air_height
] - sites_offset
site_id = self.sim.model.site_name2id('init_2b')
self.sim.model.site_pos[site_id] = object_xpos + [
self.obj_range, -self.obj_range, self.air_height
] - sites_offset
site_id = self.sim.model.site_name2id('init_3b')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.obj_range, self.obj_range, self.air_height
] - sites_offset
site_id = self.sim.model.site_name2id('init_4b')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.obj_range, -self.obj_range, self.air_height
] - sites_offset
site_id = self.sim.model.site_name2id('mark0a')
self.sim.model.site_pos[site_id] = object_xpos + [
self.target_range, self.target_range, 0.0
] - sites_offset
site_id = self.sim.model.site_name2id('mark1a')
self.sim.model.site_pos[site_id] = object_xpos + [
self.target_range, -self.target_range, 0.0
] - sites_offset
site_id = self.sim.model.site_name2id('mark2a')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.target_range, self.target_range, 0.0
] - sites_offset
site_id = self.sim.model.site_name2id('mark3a')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.target_range, -self.target_range, 0.0
] - sites_offset
site_id = self.sim.model.site_name2id('mark0b')
self.sim.model.site_pos[site_id] = object_xpos + [
self.target_range, self.target_range, self.air_height / 2
] - sites_offset
site_id = self.sim.model.site_name2id('mark1b')
self.sim.model.site_pos[site_id] = object_xpos + [
self.target_range, -self.target_range, self.air_height / 2
] - sites_offset
site_id = self.sim.model.site_name2id('mark2b')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.target_range, self.target_range, self.air_height / 2
] - sites_offset
site_id = self.sim.model.site_name2id('mark3b')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.target_range, -self.target_range, self.air_height / 2
] - sites_offset
site_id = self.sim.model.site_name2id('mark0c')
self.sim.model.site_pos[site_id] = object_xpos + [
self.target_range, self.target_range, self.air_height
] - sites_offset
site_id = self.sim.model.site_name2id('mark1c')
self.sim.model.site_pos[site_id] = object_xpos + [
self.target_range, -self.target_range, self.air_height
] - sites_offset
site_id = self.sim.model.site_name2id('mark2c')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.target_range, self.target_range, self.air_height
] - sites_offset
site_id = self.sim.model.site_name2id('mark3c')
self.sim.model.site_pos[site_id] = object_xpos + [
-self.target_range, -self.target_range, self.air_height
] - sites_offset
self.sim.step()
if self.has_object:
self.height_offset = self.sim.data.get_site_xpos('object0')[2]
