def init(self):
b = self.scene.create_actor_builder()
b.add_box_shape(size=[0.02, .5, 1])
b.add_box_visual(size=[0.02, .5, 1])
w1 = b.build(True)
w1.set_pose(Pose([-0.2, 0, 1]))
b = self.scene.create_actor_builder()
b.add_box_shape(size=[0.02, .5, 1])
b.add_box_visual(size=[0.02, .5, 1])
w2 = b.build(True)
w2.set_pose(Pose([0.2, 0, 1]))
b = self.scene.create_actor_builder()
b.add_box_shape(size=[0.25, 0.02, 1])
b.add_box_visual(size=[0.25, 0.02, 1])
w3 = b.build(True)
w3.set_pose(Pose([0, -0.4, 1]))
b = self.scene.create_actor_builder()
b.add_box_shape(size=[0.25, 0.02, 1])
b.add_box_visual(size=[0.25, 0.02, 1])
w4 = b.build(True)
w4.set_pose(Pose([0, 0.4, 1]))
for i in range(1500):
self.scene.step()
self.scene.remove_actor(w1)
self.scene.remove_actor(w2)
self.scene.remove_actor(w3)
self.scene.remove_actor(w4)
def reset(self):
self.total_box_genreated += 10
self.local_total_timesteps = 0
if self.left_robot:
self.scene.remove_articulation(self.left_robot.robot)
self.scene.remove_articulation(self.right_robot.robot)
base_size = np.array([0.015, 0.07, 0.07])
size = base_size + np.random.random(3) * np.array([0.01, 0.06, 0.06])
self.left_robot = Robot(
self.load_robot(Pose([-.5, .25, 0.6], axangle2quat([0, 0, 1], -np.pi / 2)), size))
self.left_robot.robot.set_qpos([1.5, 1, 0, -1, 0, 0, 0])
self.right_robot = Robot(
self.load_robot(Pose([-.5, -.25, 0.6], axangle2quat([0, 0, 1], np.pi / 2)), size))
self.right_robot.robot.set_qpos([-1.5, 1, 0, -1, 0, 0, 0])
self.create_bin([-1.2 + np.random.random() * 0.1,
np.random.random() * 0.2 - 0.1, 0.6],
np.random.random() * np.pi, [
0.2 + np.random.random() * 0.05, 0.3 + np.random.random() * 0.05,
0.4 + np.random.random() * 0.05
], 0.015 + np.random.random() * 0.05)
for d in self.boxes:
d.set_pose(Pose([np.random.random() * 0.2 - 0.1, np.random.random() * 0.4 - 0.2, 2]))
self.init()
self.scene.update_render()
def setup_boxes(self):
object_material = sim.create_physical_material(1, 1, 0.1)
size = 0.02
b = self.scene.create_actor_builder()
b.add_box_shape(size=[size] * 3, material=object_material)
b.add_box_visual(size=[size] * 3, color=[1, 0, 0])
red_box = b.build()
red_box.set_pose(Pose([0.4, 0, size]))
red_box.set_name('red')
b = self.scene.create_actor_builder()
b.add_box_shape(size=[size] * 3, material=object_material)
b.add_box_visual(size=[size] * 3, color=[0, 1, 0])
green_box = b.build()
green_box.set_pose(Pose([0.4, 0.2, size]))
green_box.set_name('green')
b = self.scene.create_actor_builder()
b.add_box_shape(size=[size] * 3, material=object_material)
b.add_box_visual(size=[size] * 3, color=[0, 0, 1])
blue_box = b.build()
blue_box.set_pose(Pose([0.4, -0.2, size]))
blue_box.set_name('blue')
self.boxes = red_box, green_box, blue_box
def __init__(self, frame_skip, timestep=0.01, gravity=(0, 0, -9.8)):
self.frame_skip = frame_skip
self.timestep = timestep
self._engine = sapien.Engine()
self._renderer = sapien.VulkanRenderer(True)
self._engine.set_renderer(self._renderer)
self.sim = self._engine.create_scene()
self.sim.set_timestep(timestep)
self.viewer_setup()
self.model, self._init_state = self._load_model()
self._dof = self.model.dof
self._root = self.model.get_links()[0]
# Unlike MuJoCo MJCF, the actuator information is not defined in the xml file
# So you should define it explicitly in Python
from gym.spaces.box import Box
actuator_info = self._load_actuator()
self._actuator_name, _actuator_idx, low, high = list(
zip(*actuator_info))
self._actuator_idx = np.array(_actuator_idx)
self.action_spec = Box(low=np.array(low),
high=np.array(high),
dtype=np.float32)
near, far = 0.1, 100
camera_mount_actor = self.sim.create_actor_builder().build(
is_kinematic=True)
camera = self.sim.add_mounted_camera(
"first_camera",
camera_mount_actor,
Pose(),
640,
480,
0,
np.deg2rad(35),
near,
far,
)
pos = np.array([-2, -2, 3])
forward = -pos / np.linalg.norm(pos)
left = np.cross([0, 0, 1], forward)
left = left / np.linalg.norm(left)
up = np.cross(forward, left)
mat44 = np.eye(4)
mat44[:3, :3] = np.array([forward, left, up]).T
mat44[:3, 3] = pos
camera_mount_actor.set_pose(Pose.from_transformation_matrix(mat44))
self.camera = camera
def load_robot(self, pose, size, thickness=0.003, offset=0.01):
x, y, z = size
lb = self.robot_builder.get_link_builders()
lb[9].remove_all_shapes()
lb[9].remove_all_visuals()
lb[9].add_box_shape(Pose([-x / 2, 0, z / 2 + offset]),
[thickness / 2, y / 2, z / 2])
lb[9].add_box_visual_complex(Pose([-x / 2, 0, z / 2 + offset]),
[thickness / 2, y / 2, z / 2], steel)
lb[9].add_box_shape(Pose([0, y / 2, z / 2 + offset]),
[x / 2 + thickness / 2, thickness / 2, z / 2])
lb[9].add_box_visual_complex(
Pose([0, y / 2, z / 2 + offset]),
[x / 2 + thickness / 2, thickness / 2, z / 2], steel)
lb[9].add_box_shape(Pose([0, -y / 2, z / 2 + offset]),
[x / 2 + thickness / 2, thickness / 2, z / 2])
lb[9].add_box_visual_complex(
Pose([0, -y / 2, z / 2 + offset]),
[x / 2 + thickness / 2, thickness / 2, z / 2], steel)
lb[9].add_box_shape(
Pose([0, 0, offset]),
[x / 2 + thickness / 2, y / 2 + thickness / 2, thickness / 2])
lb[9].add_box_visual_complex(Pose([
0, 0, offset
]), [x / 2 + thickness / 2, y / 2 + thickness / 2, thickness / 2],
steel)
r = self.robot_builder.build(True)
r.name = "robot"
r.set_root_pose(pose)
return r
def build_room(self):
b = self.scene.create_actor_builder()
b.add_box_shape(Pose([0, 0, -0.1]), [2.5, 2.5, 0.1])
b.add_box_shape(Pose([0, -2.5, 1.5]), [2.5, 0.1, 1.5])
b.add_box_shape(Pose([0, 2.5, 1.5]), [2.5, 0.1, 1.5])
b.add_box_shape(Pose([-2.5, 0, 1.5]), [0.1, 2.5, 1.5])
b.add_box_shape(Pose([2.5, 0, 1.5]), [0.1, 2.5, 1.5])
b.add_box_visual(Pose([0, 0, -0.1]), [2.5, 2.5, 0.1], [0.2, 0.2, 0.2])
b.add_box_visual(Pose([0, -2.5, 1.5]), [2.5, 0.1, 1.5], [0.8, 0.8, 0.8])
b.add_box_visual(Pose([0, 2.5, 1.5]), [2.5, 0.1, 1.5], [0.8, 0.8, 0.8])
b.add_box_visual(Pose([-2.5, 0, 1.5]), [0.1, 2.5, 1.5], [0.8, 0.8, 0.8])
b.add_box_visual(Pose([2.5, 0, 1.5]), [0.1, 2.5, 1.5], [0.8, 0.8, 0.8])
self.room = b.build_static(name="room")
def init(self, env: Env) -> None:
if self._initialized:
print(f"Task: {TASK_NAME} already initialized")
return
# add box
self._box = env.agents[BOX_NAME]
assert self._box is not None and isinstance(self._box, Box), \
f"{BOX_NAME} does not exist in env"
self._box_height_thresh = self._box.observation['pose'].p[
2] + BOX_HEIGHT_THRESHOLD
# add arm
self._arm = env.agents[ARM_NAME]
assert self._arm is not None and isinstance(self._arm, PandaArm), \
f"{ARM_NAME} does not exist in env"
# build goal
self._goal = build_goal(env.scene, self._box.box_size)
self._goal.set_pose(Pose(GOAl_POS))
self._box_valid_push = True
self._suceeded = False
self._tracking = True
self._failed_reason = None
self._parameters = {
"box_size": self._box.box_size,
"box_tilt_threshold_rad": TILT_THRESHHOLD,
"box_at_goal_threshold_meter": BOX_AT_GOAL_THRESHOLD,
"box_height_threshold_global": self._box_height_thresh,
"goal_pos": GOAl_POS,
}
self._initialized = True
print(f"Task: {TASK_NAME} initialized")
def init(self, env: Env):
if self._initialized:
print(f"{self.name} already initialized")
return
# load arm
loader = env.scene.create_urdf_loader()
loader.fix_root_link = True
arm_path = os.path.join(config.ASSET_DIR, "Arm/panda.urdf")
print()
if not self._pusher:
self._robot = loader.load(arm_path)
else:
builder = loader.load_file_as_articulation_builder(arm_path)
lb = builder.get_link_builders()
lb[-1].remove_all_shapes()
lb[-1].remove_all_visuals()
lb[-2].remove_all_shapes()
lb[-2].remove_all_visuals()
lb[-3].remove_all_shapes()
lb[-3].remove_all_visuals()
pusher_size = [0.06, 0.06, 0.03]
pusher_pose = Pose([0,0,0.03])
material = env._sim.create_physical_material(0.9, 0.9, 0)
lb[-3].add_box_shape(pusher_pose, pusher_size, material=material)
lb[-3].add_box_visual_complex(pusher_pose, pusher_size)
self._robot = builder.build(True)
self.dof = self._robot.dof
# adjust damping
for joint, d in zip(self._robot.get_joints(), np.array([self._damping] * self._robot.dof)):
joint.set_drive_property(stiffness=0, damping=d)
self._robot.set_pose(INI_POSE)
self._robot.set_qpos(INI_QPOS)
self._robot.set_name(self.name)
self._active_joints = [j for j in self._robot.get_joints() if j.get_dof() == 1]
qpos = np.array([0, 0, 0, -1.5, 0, 1.5, 0.7, 0.4, 0.4])
self._robot.set_qpos(qpos)
self._robot.set_qvel([0] * self._robot.dof)
self.set_action(qpos, [0] * self._robot.dof, self._robot.compute_passive_force())
# set specs
self._action_spec = {
"qlimits": self._robot.get_qlimits()
}
self._observation_spec = None
self._initialized = True
print(f"CREATED: {self.name} created")
def __init__(self, env, near=0.1, far=100.0, image_size=448, dist=5.0, \
phi=np.pi/5, theta=np.pi, fov=35, random_position=False, fixed_position=False):
builder = env.scene.create_actor_builder()
camera_mount_actor = builder.build(is_kinematic=True)
self.env = env
# set camera intrinsics
self.camera = env.scene.add_mounted_camera('camera', camera_mount_actor, Pose(), \
image_size, image_size, 0, np.deg2rad(fov), near, far)
# set camera extrinsics
if random_position:
theta = np.random.random() * np.pi*2
phi = (np.random.random()+1) * np.pi/6
if fixed_position:
#theta = -np.pi/10
#theta = -np.pi/8
theta = np.pi
phi = np.pi/10
pos = np.array([dist*np.cos(phi)*np.cos(theta), \
dist*np.cos(phi)*np.sin(theta), \
dist*np.sin(phi)])
forward = -pos / np.linalg.norm(pos)
left = np.cross([0, 0, 1], forward)
left = left / np.linalg.norm(left)
up = np.cross(forward, left)
mat44 = np.eye(4)
mat44[:3, :3] = np.vstack([forward, left, up]).T
mat44[:3, 3] = pos # mat44 is cam2world
mat44[0, 3] += env.object_position_offset
self.mat44 = mat44
camera_mount_actor.set_pose(Pose.from_transformation_matrix(mat44))
# log parameters
self.near = near
self.far = far
self.dist = dist
self.theta = theta
self.phi = phi
self.pos = pos
def random_pose_circle(theta_range=None, len=None):
if len is None:
len = [0.3, 0.6]
if theta_range is None:
theta_range = [0, 2 * np.pi]
theta = np.random.uniform(low=theta_range[0], high=theta_range[1])
r = np.random.uniform(low=len[0], high=len[1])
x = r * np.cos(theta)
y = r * np.sin(theta)
return Pose(np.array([x, y, -0.99]))
def load_object(self, urdf, material, state='closed'):
loader = self.scene.create_urdf_loader()
self.object = loader.load(urdf, {"material": material})
#self.object = loader.load(urdf, material)
pose = Pose([self.object_position_offset, 0, 0], [1, 0, 0, 0])
self.object.set_root_pose(pose)
# compute link actor information
self.all_link_ids = [l.get_id() for l in self.object.get_links()]
self.movable_link_ids = []
for j in self.object.get_joints():
if j.get_dof() == 1:
self.movable_link_ids.append(j.get_child_link().get_id())
if self.flog is not None:
self.flog.write('All Actor Link IDs: %s\n' %
str(self.all_link_ids))
self.flog.write('All Movable Actor Link IDs: %s\n' %
str(self.movable_link_ids))
# set joint property
for joint in self.object.get_joints():
joint.set_drive_property(stiffness=0, damping=10)
# set initial qpos
joint_angles = []
self.joint_angles_lower = []
self.joint_angles_upper = []
for j in self.object.get_joints():
if j.get_dof() == 1:
l = process_angle_limit(j.get_limits()[0, 0])
self.joint_angles_lower.append(float(l))
r = process_angle_limit(j.get_limits()[0, 1])
self.joint_angles_upper.append(float(r))
if state == 'closed':
joint_angles.append(float(l))
elif state == 'open':
joint_angles.append(float(r))
elif state == 'random-middle':
joint_angles.append(float(get_random_number(l, r)))
elif state == 'random-closed-middle':
if np.random.random() < 0.5:
joint_angles.append(float(get_random_number(l, r)))
else:
joint_angles.append(float(l))
else:
raise ValueError('ERROR: object init state %s unknown!' %
state)
self.object.set_qpos(joint_angles)
return joint_angles
def plot_figure(up, forward):
# cam to world
up = mat33 @ up
forward = mat33 @ forward
up = np.array(up, dtype=np.float32)
forward = np.array(forward, dtype=np.float32)
left = np.cross(up, forward)
left /= np.linalg.norm(left)
forward = np.cross(left, up)
forward /= np.linalg.norm(forward)
rotmat = np.eye(4).astype(np.float32)
rotmat[:3, 0] = forward
rotmat[:3, 1] = left
rotmat[:3, 2] = up
rotmat[:3, 3] = position_world - up * 0.1
pose = Pose().from_transformation_matrix(rotmat)
robot.robot.set_root_pose(pose)
env.render()
rgb_final_pose, _, _, _ = cam.get_observation()
fimg = Image.fromarray((rgb_final_pose*255).astype(np.uint8))
fimg.save(os.path.join(result_dir, 'action.png'))
def before_step(self, env, task):
robot: PandaArm = env.agents[ARM_NAME]
box: Box = env.agents[BOX_NAME]
if self.phase is Phase.rotate_toward_box:
if self.init_control is True:
box2base = robot.observation['poses'][0].inv(
) * box.observation['pose']
p = box2base.p
theta = np.arctan(p[1] / p[0])
if p[0] < 0 and p[1] < 0:
theta -= np.pi
elif p[0] < 0 and p[1] > 0:
theta += np.pi
self.init_control = False
self.tg_pose = Pose(robot.observation['poses'][0].p,
euler.euler2quat(0, 0, theta))
self.drive_to_pose(robot,
self.tg_pose,
joint_index=1,
theta_thresh=1e-4,
theta_abs_thresh=1e-5)
if self.drive[robot] is False:
self.phase = Phase.move_above_box
self.prep_drive(robot)
elif self.phase is Phase.move_above_box:
if self.init_control is True:
self.tg_pose = robot.observation['poses'][-1]
self.tg_pose.set_p(box.observation['pose'].p)
self.tg_pose = Pose([0, 0, 0.2 + box.box_size]) * self.tg_pose
self.init_control = False
self.drive_to_pose(robot,
self.tg_pose,
override=([-1, -2], [0, 0.6], [0, 0]))
if self.drive[robot] is False:
self.phase = Phase.tap_down
self.prep_drive(robot)
elif self.phase is Phase.tap_down:
if self.init_control is True:
self.tg_pose = robot.observation['poses'][-1]
self.tg_pose.set_p(box.observation['pose'].p)
self.tg_pose = Pose([0, 0, 0.1 + box.box_size]) * self.tg_pose
self.init_control = False
self.drive_to_pose(robot,
self.tg_pose,
override=([-1, -2], [0, 0.6], [0, 0]),
max_v=5e-2)
if task.get_trajectory_status()['touched'] is True:
self.phase = Phase.tap_up
self.prep_drive(robot)
elif self.phase is Phase.tap_up:
if self.init_control is True:
self.tg_pose = robot.observation['poses'][-1]
self.tg_pose.set_p(box.observation['pose'].p)
self.tg_pose = Pose([0, 0, 0.2 + box.box_size]) * self.tg_pose
self.init_control = False
self.drive_to_pose(robot,
self.tg_pose,
override=([-1, -2], [0, 0.6], [0, 0]))
if self.drive[robot] is False:
self.phase = Phase.evaluate
self.prep_drive(robot)
elif self.phase is Phase.evaluate:
print(task.get_trajectory_status())
self.phase = Phase.done
def create_ant_builder(scene):
builder = scene.create_articulation_builder()
body = builder.create_link_builder()
body.add_sphere_shape(Pose(), 0.25)
body.add_sphere_visual_complex(Pose(), 0.25, copper)
body.add_capsule_shape(Pose([0.141, 0, 0]), 0.08, 0.141)
body.add_capsule_visual_complex(Pose([0.141, 0, 0]), 0.08, 0.141, copper)
body.add_capsule_shape(Pose([-0.141, 0, 0]), 0.08, 0.141)
body.add_capsule_visual_complex(Pose([-0.141, 0, 0]), 0.08, 0.141, copper)
body.add_capsule_shape(Pose([0, 0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08,
0.141)
body.add_capsule_visual_complex(
Pose([0, 0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141, copper)
body.add_capsule_shape(Pose([0, -0.141, 0], aa([0, 0, 1], np.pi / 2)),
0.08, 0.141)
body.add_capsule_visual_complex(
Pose([0, -0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141, copper)
body.set_name("body")
l1 = builder.create_link_builder(body)
l1.set_name("l1")
l1.set_joint_name("j1")
l1.set_joint_properties(sapien.ArticulationJointType.REVOLUTE,
[[-0.5236, 0.5236]],
Pose([0.282, 0, 0], [0.7071068, 0, 0.7071068, 0]),
Pose([0.141, 0, 0],
[-0.7071068, 0, 0.7071068, 0]), 0.1)
l1.add_capsule_shape(Pose(), 0.08, 0.141)
l1.add_capsule_visual_complex(Pose(), 0.08, 0.141, copper)
l2 = builder.create_link_builder(body)
l2.set_name("l2")
l2.set_joint_name("j2")
l2.set_joint_properties(
sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]],
Pose([-0.282, 0, 0], [0, -0.7071068, 0, 0.7071068]),
Pose([0.141, 0, 0], [-0.7071068, 0, 0.7071068, 0]), 0.1)
l2.add_capsule_shape(Pose(), 0.08, 0.141)
l2.add_capsule_visual_complex(Pose(), 0.08, 0.141, copper)
l3 = builder.create_link_builder(body)
l3.set_name("l3")
l3.set_joint_name("j3")
l3.set_joint_properties(sapien.ArticulationJointType.REVOLUTE,
[[-0.5236, 0.5236]],
Pose([0, 0.282, 0], [0.5, -0.5, 0.5, 0.5]),
Pose([0.141, 0, 0],
[0.7071068, 0, -0.7071068, 0]), 0.1)
l3.add_capsule_shape(Pose(), 0.08, 0.141)
l3.add_capsule_visual_complex(Pose(), 0.08, 0.141, copper)
l4 = builder.create_link_builder(body)
l4.set_name("l4")
l4.set_joint_name("j4")
l4.set_joint_properties(sapien.ArticulationJointType.REVOLUTE,
[[-0.5236, 0.5236]],
Pose([0, -0.282, 0], [0.5, 0.5, 0.5, -0.5]),
Pose([0.141, 0, 0],
[0.7071068, 0, -0.7071068, 0]), 0.1)
l4.add_capsule_shape(Pose(), 0.08, 0.141)
l4.add_capsule_visual_complex(Pose(), 0.08, 0.141, copper)
f1 = builder.create_link_builder(l1)
f1.set_name("f1")
f1.set_joint_name("j11")
f1.set_joint_properties(sapien.ArticulationJointType.REVOLUTE,
[[0.5236, 1.222]],
Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]),
Pose([0.282, 0, 0],
[0, 0.7071068, 0.7071068, 0]), 0.1)
f1.add_capsule_shape(Pose(), 0.08, 0.282)
f1.add_capsule_visual_complex(Pose(), 0.08, 0.282, copper)
f2 = builder.create_link_builder(l2)
f2.set_name("f2")
f2.set_joint_name("j21")
f2.set_joint_properties(sapien.ArticulationJointType.REVOLUTE,
[[0.5236, 1.222]],
Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]),
Pose([0.282, 0, 0],
[0, 0.7071068, 0.7071068, 0]), 0.1)
f2.add_capsule_shape(Pose(), 0.08, 0.282)
f2.add_capsule_visual_complex(Pose(), 0.08, 0.282, copper)
f3 = builder.create_link_builder(l3)
f3.set_name("f3")
f3.set_joint_name("j31")
f3.set_joint_properties(sapien.ArticulationJointType.REVOLUTE,
[[0.5236, 1.222]],
Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]),
Pose([0.282, 0, 0],
[0, 0.7071068, 0.7071068, 0]), 0.1)
f3.add_capsule_shape(Pose(), 0.08, 0.282)
f3.add_capsule_visual_complex(Pose(), 0.08, 0.282, copper)
f4 = builder.create_link_builder(l4)
f4.set_name("f4")
f4.set_joint_name("j41")
f4.set_joint_properties(sapien.ArticulationJointType.REVOLUTE,
[[0.5236, 1.222]],
Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]),
Pose([0.282, 0, 0],
[0, 0.7071068, 0.7071068, 0]), 0.1)
f4.add_capsule_shape(Pose(), 0.08, 0.282)
f4.add_capsule_visual_complex(Pose(), 0.08, 0.282, copper)
return builder
return builder
render_controller.show_window()
s0 = sim.create_scene()
s0.add_ground(-3)
s0.set_timestep(1 / 240)
s0.set_ambient_light([0.5, 0.5, 0.5])
s0.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])
ant_builder = create_ant_builder(s0)
ant = ant_builder.build()
ant.set_root_pose(Pose([0, 0, 2]))
render_controller.set_camera_position(-5, 0, 2)
render_controller.set_current_scene(s0)
render_controller.focus(ant.get_links()[0])
for j in ant.get_joints():
j.set_friction(0)
s0.step()
ant.set_qpos([0, 0, 0, 0, 0.7, 0.7, 0.7, 0.7])
ant.set_qvel([0] * 8)
f = [0.1] * 8
acc = ant.compute_forward_dynamics([0.1] * 8)
print(acc)
def success_test(self):
self.scene.remove_articulation(self.robot)
self.boxes[0].set_pose(Pose([0.6, 0, 0.03]))
self.boxes[1].set_pose(Pose([0.61, 0.01, 0.15]))
self.boxes[2].set_pose(Pose([0.59, -0.01, 0.3]))
def act(self, env: FinalEnv, current_timestep: int):
r1, r2, c1, c2, c3, c4 = env.get_agents()
pf_left = f = r1.get_compute_functions()['passive_force'](True, True,
False)
pf_right = f = r2.get_compute_functions()['passive_force'](True, True,
False)
if self.phase == 0:
t1 = [2, 1, 0, -1.5, -1, 1, -2]
t2 = [-2, 1, 0, -1.5, 1, 1, -2]
r1.set_action(t1, [0] * 7, pf_left)
r2.set_action(t2, [0] * 7, pf_right)
if np.allclose(r1.get_observation()[0],
t1, 0.05, 0.05) and np.allclose(
r2.get_observation()[0], t2, 0.05, 0.05):
self.phase = 1
self.counter = 0
self.selected_x = None
if self.phase == 1:
self.counter += 1
if (self.counter == 1):
selected = self.pick_box(c4)
self.selected_x = selected[0]
if self.selected_x is None:
return False
target_pose_left = Pose([self.selected_x, 0.5, 0.67],
euler2quat(np.pi, -np.pi / 3, -np.pi / 2))
self.diff_drive(r1, 9, target_pose_left)
target_pose_right = Pose([self.selected_x, -0.5, 0.6],
euler2quat(np.pi, -np.pi / 3, np.pi / 2))
self.diff_drive(r2, 9, target_pose_right)
if self.counter == 2000 / 5:
self.phase = 2
self.counter = 0
pose = r1.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = 0.07
self.pose_left = Pose(p, q)
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = -0.07
self.pose_right = Pose(p, q)
if self.phase == 2:
self.counter += 1
self.diff_drive(r1, 9, self.pose_left)
self.diff_drive(r2, 9, self.pose_right)
if self.counter == 2000 / 5:
self.phase = 3
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
p[2] += 0.2
self.pose_right = Pose(p, q)
pose = r1.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = 0.5
q = euler2quat(np.pi, -np.pi / 2, -np.pi / 2)
self.pose_left = Pose(p, q)
self.counter = 0
if self.phase == 3:
self.counter += 1
self.diff_drive(r1, 9, self.pose_left)
self.diff_drive(r2, 9, self.pose_right)
if self.counter == 200 / 5:
self.phase = 4
self.counter = 0
if self.phase == 4:
self.counter += 1
if (self.counter < 3000 / 5):
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
q = euler2quat(np.pi, -np.pi / 1.5, quat2euler(q)[2])
self.diff_drive2(r2, 9, Pose(p, q), [4, 5, 6],
[0, 0, 0, -1, 0], [0, 1, 2, 3, 4])
elif (self.counter < 6000 / 5):
pose = r2.get_observation()[2][9]
p, q = pose.p, pose.q
q = euler2quat(np.pi, -np.pi / 1.5, quat2euler(q)[2])
self.diff_drive2(r2, 9, Pose(p, q), [4, 5, 6],
[0, 0, 1, -1, 0], [0, 1, 2, 3, 4])
elif (self.counter < 9000 / 5):
p = [-1, 0, 1.5]
q = euler2quat(0, -np.pi / 1.5, 0)
self.diff_drive(r2, 9, Pose(p, q))
else:
self.phase = 0
def create_ant_builder(scene):
from transforms3d.quaternions import axangle2quat as aa
builder = scene.create_articulation_builder()
body = builder.create_link_builder()
body.add_sphere_shape(Pose(), 0.25)
body.add_sphere_visual(Pose(), 0.25)
body.add_capsule_shape(Pose([0.141, 0, 0]), 0.08, 0.141)
body.add_capsule_visual(Pose([0.141, 0, 0]), 0.08, 0.141)
body.add_capsule_shape(Pose([-0.141, 0, 0]), 0.08, 0.141)
body.add_capsule_visual(Pose([-0.141, 0, 0]), 0.08, 0.141)
body.add_capsule_shape(Pose([0, 0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141)
body.add_capsule_visual(Pose([0, 0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141)
body.add_capsule_shape(Pose([0, -0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141)
body.add_capsule_visual(Pose([0, -0.141, 0], aa([0, 0, 1], np.pi / 2)), 0.08, 0.141)
body.set_name("body")
l1 = builder.create_link_builder(body)
l1.set_name("l1")
l1.set_joint_name("hip_1")
l1.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]],
Pose([0.282, 0, 0], [0.7071068, 0, 0.7071068, 0]),
Pose([0.141, 0, 0], [-0.7071068, 0, 0.7071068, 0]), 0.1)
l1.add_capsule_shape(Pose(), 0.08, 0.141)
l1.add_capsule_visual(Pose(), 0.08, 0.141)
l2 = builder.create_link_builder(body)
l2.set_name("l2")
l2.set_joint_name("hip_2")
l2.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]],
Pose([-0.282, 0, 0], [0, -0.7071068, 0, 0.7071068]),
Pose([0.141, 0, 0], [-0.7071068, 0, 0.7071068, 0]), 0.1)
l2.add_capsule_shape(Pose(), 0.08, 0.141)
l2.add_capsule_visual(Pose(), 0.08, 0.141)
l3 = builder.create_link_builder(body)
l3.set_name("l3")
l3.set_joint_name("hip_3")
l3.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]],
Pose([0, 0.282, 0], [0.5, -0.5, 0.5, 0.5]),
Pose([0.141, 0, 0], [0.7071068, 0, -0.7071068, 0]), 0.1)
l3.add_capsule_shape(Pose(), 0.08, 0.141)
l3.add_capsule_visual(Pose(), 0.08, 0.141)
l4 = builder.create_link_builder(body)
l4.set_name("l4")
l4.set_joint_name("hip_4")
l4.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[-0.5236, 0.5236]],
Pose([0, -0.282, 0], [0.5, 0.5, 0.5, -0.5]),
Pose([0.141, 0, 0], [0.7071068, 0, -0.7071068, 0]), 0.1)
l4.add_capsule_shape(Pose(), 0.08, 0.141)
l4.add_capsule_visual(Pose(), 0.08, 0.141)
f1 = builder.create_link_builder(l1)
f1.set_name("f1")
f1.set_joint_name("ankle_1")
f1.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]],
Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]),
Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1)
f1.add_capsule_shape(Pose(), 0.08, 0.282)
f1.add_capsule_visual(Pose(), 0.08, 0.282)
f2 = builder.create_link_builder(l2)
f2.set_name("f2")
f2.set_joint_name("ankle_2")
f2.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]],
Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]),
Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1)
f2.add_capsule_shape(Pose(), 0.08, 0.282)
f2.add_capsule_visual(Pose(), 0.08, 0.282)
f3 = builder.create_link_builder(l3)
f3.set_name("f3")
f3.set_joint_name("ankle_3")
f3.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]],
Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]),
Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1)
f3.add_capsule_shape(Pose(), 0.08, 0.282)
f3.add_capsule_visual(Pose(), 0.08, 0.282)
f4 = builder.create_link_builder(l4)
f4.set_name("f4")
f4.set_joint_name("ankle_4")
f4.set_joint_properties(sapien.ArticulationJointType.REVOLUTE, [[0.5236, 1.222]],
Pose([-0.141, 0, 0], [0, 0.7071068, 0.7071068, 0]),
Pose([0.282, 0, 0], [0, 0.7071068, 0.7071068, 0]), 0.1)
f4.add_capsule_shape(Pose(), 0.08, 0.282)
f4.add_capsule_visual(Pose(), 0.08, 0.282)
return builder
def build_camreas(self):
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/camera.dae')
cam_front = b.build(True, name="camera")
pitch = np.deg2rad(-15)
cam_front.set_pose(
Pose([1, 0, 1], qmult(axangle2quat([0, 0, 1], np.pi), axangle2quat([0, -1, 0], pitch))))
c = self.scene.add_mounted_camera('front_camera', cam_front, Pose(), 640, 480, 0, np.deg2rad(45), 0.1,
10)
self.front_camera = Camera(c)
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1]))
b.build(True, name="tripod").set_pose(Pose([1, 0, 1]))
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/camera.dae')
cam_left = b.build(True, name="camera")
pitch = np.deg2rad(-15)
cam_left.set_pose(
Pose([0, 1.5, 1], qmult(axangle2quat([0, 0, 1], -np.pi / 2), axangle2quat([0, -1, 0], pitch))))
c = self.scene.add_mounted_camera('left_camera', cam_left, Pose(), 640, 480, 0, np.deg2rad(45), 0.1, 10)
self.left_camera = Camera(c)
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1]))
b.build(True, name="tripod").set_pose(Pose([0, 1.5, 1]))
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/camera.dae')
cam_right = b.build(True, name="camera")
pitch = np.deg2rad(-15)
cam_right.set_pose(
Pose([0, -1.5, 1], qmult(axangle2quat([0, 0, 1], np.pi / 2), axangle2quat([0, -1, 0], pitch))))
c = self.scene.add_mounted_camera('right_camera', cam_right, Pose(), 640, 480, 0, np.deg2rad(45), 0.1, 10)
self.right_camera = Camera(c)
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1]))
b.build(True, name="tripod").set_pose(Pose([0, -1.5, 1]))
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/camera.dae')
cam_top = b.build(True, name="camera")
pitch = np.deg2rad(-90)
cam_top.set_pose(Pose([-0.5, 0, 3], axangle2quat([0, -1, 0], pitch)))
c = self.scene.add_mounted_camera('top_camera', cam_top, Pose(), 640, 480, 0, np.deg2rad(45), 0.1, 10)
self.top_camera = Camera(c)
def before_step(self, env, task: DexPushTask):
robot: PandaArm = env.agents[ARM_NAME]
box: Box = env.agents[BOX_NAME]
if self.phase is Phase.rotate_toward_box:
if self.init_control is True:
box2base = robot.observation['poses'][0].inv() * box.observation['pose']
p = box2base.p
theta = np.arctan(p[1] / p[0])
if p[0] < 0 and p[1] < 0:
theta -= np.pi
elif p[0] < 0 and p[1] > 0:
theta += np.pi
self.init_control = False
self.tg_pose = Pose(robot.observation['poses'][0].p, euler.euler2quat(0, 0, theta))
self.drive_to_pose(robot, self.tg_pose, joint_index=1, theta_thresh=1e-4, theta_abs_thresh=1e-5)
if self.drive[robot] is False:
self.phase = Phase.move2above
self.prep_drive(robot)
elif self.phase is Phase.move2above:
if self.init_control is True:
box2wd = box.observation['pose']
goal2box = box2wd.inv() * Pose(self.goal2wd)
robot2box = box2wd.inv() * robot.observation['poses'][0]
direct = np.array([0, 0, 0])
if np.min(np.abs(goal2box.p)) > 5e-3:
indx = np.argmin(np.abs(robot2box.p[:2]))
if np.abs(goal2box.p[indx]) < 8e-4:
indx = 1 - indx
else:
indx = np.argmax(np.abs(goal2box.p[:2]))
direct[indx] = 1
dist = goal2box.p @ direct
dist = np.min((abs(dist), 0.2)) * np.sign(dist)
quat = Pose([0]*3, self.up_right_q)
if direct[1] == 1:
quat = quat * Pose([0]*3, euler.euler2quat(0, 0, np.pi/2))
ee2pt = Pose([0,0,0.1], quat.q)
gripper_width = 0.013
self.plan2wd = box2wd * Pose(-direct * np.sign(dist) * (box.box_size + gripper_width) + dist * direct) * ee2pt
self.push2wd = box2wd * Pose(-direct * np.sign(dist) * (box.box_size + gripper_width)) * ee2pt
self.tg_pose = Pose([0,0,0.1]) * box2wd * Pose(-direct * np.sign(dist) * (box.box_size + gripper_width + 0.02)) * ee2pt
self.init_control = False
self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0], [0, 0]), dist_abs_thresh=1e-6, dist_thresh=1e-6, theta_abs_thresh=1e-3, theta_thresh=1e-3)
if self.drive[robot] is False:
self.phase = Phase.move2box
self.prep_drive(robot)
elif self.phase is Phase.move2box:
if self.init_control is True:
self.tg_pose = self.push2wd
self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0], [0, 0]), dist_abs_thresh=1e-5, dist_thresh=1e-5, theta_abs_thresh=1e-3, theta_thresh=1e-3)
if self.drive[robot] is False:
self.prep_drive(robot)
self.phase = Phase.push2goal
elif self.phase is Phase.push2goal:
if self.init_control is True:
self.tg_pose = self.plan2wd
self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0], [0, 0]), dist_abs_thresh=1e-5, dist_thresh=1e-5, theta_abs_thresh=1e-3, theta_thresh=1e-3)
if self.drive[robot] is False or task.get_trajectory_status()['succeeded'] is True:
self.prep_drive(robot)
self.phase = Phase.moveUp
elif self.phase is Phase.moveUp:
if self.init_control is True:
self.tg_pose = Pose([0,0,0.1]) * robot.observation['poses'][-3]
self.init_control = False
self.drive_to_pose(robot, self.tg_pose, override=([-1, -2], [0, 0], [0, 0]), dist_abs_thresh=1e-4)
if self.drive[robot] is False:
self.prep_drive(robot)
status = task.get_trajectory_status()
if status['succeeded'] is False and status['valid_push'] is True:
self.phase = Phase.move2above
else:
self.phase = Phase.evaluate
elif self.phase is Phase.evaluate:
print(task.get_trajectory_status())
self.phase = Phase.done
sim.set_renderer(renderer)
scene = sim.create_scene()
scene.set_timestep(1 / 60)
scene.set_ambient_light([0.5, 0.5, 0.5])
scene.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])
scene.add_point_light([1, 2, 2], [1, 1, 1])
scene.add_point_light([1, -2, 2], [1, 1, 1])
scene.add_point_light([-1, 0, 1], [1, 1, 1])
near, far = 0.1, 100
camera_mount_actor = scene.create_actor_builder().build(is_kinematic=True)
camera = scene.add_mounted_camera(
"first_camera",
camera_mount_actor,
Pose(),
640,
480,
np.deg2rad(35),
np.deg2rad(35),
near,
far,
)
pos = np.array([-2, -2, 3])
forward = -pos / np.linalg.norm(pos)
left = np.cross([0, 0, 1], forward)
left = left / np.linalg.norm(left)
up = np.cross(forward, left)
mat44 = np.eye(4)
mat44[:3, :3] = np.array([forward, left, up]).T
"panda_leftfinger": {
"patch_radius": 0.5
},
"panda_rightfinger": {
"shape": {
0: {
"patch_radius": 0.4
}
}
},
}
}
robot = loader.load("../assets_local/robot/panda.urdf", config)
model = robot.create_pinocchio_model()
targetPose = Pose([-0.01, -0.39, 0.96], [0.21, 0.73, 0.41, 0.5])
result, success, error = model.compute_inverse_kinematics(9, targetPose)
print('success: ', success)
print('error: ', error)
# robot.set_qpos(result)
loader.load("../assets/robot/sapien_gripper.urdf")
render_controller.show_window()
render_controller.set_camera_position(-5, 0, 0)
render_controller.set_current_scene(scene)
steps = 0
while not render_controller.should_quit:
scene.update_render()
for i in range(4):
renderer = sapien.OptifuserRenderer()
sim.set_renderer(renderer)
controller = sapien.OptifuserController(renderer)
controller.show_window()
s0 = sim.create_scene()
s0.add_ground(-1)
s0.set_timestep(1 / 60)
s0.set_ambient_light([0.5, 0.5, 0.5])
s0.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])
builder = s0.create_actor_builder()
# builder.add_box_shape()
# builder.add_box_visual()
builder.add_multiple_convex_shapes_from_file(
'/home/fx/source/py-vhacd/example/decomposed.obj')
builder.add_visual_from_file('/home/fx/source/py-vhacd/example/decomposed.obj')
actor = builder.build()
actor.set_pose(Pose([0, 0, 2]))
controller.set_current_scene(s0)
while not controller.should_quit:
s0.update_render()
s0.step()
controller.render()
def create_bin(self, p, r, size, thickness):
if self.bin is not None:
self.scene.remove_actor(self.bin)
self.bin_size = size
b = self.scene.create_actor_builder()
b.add_box_shape(Pose([0, 0, thickness / 2]), [size[0] / 2, size[1] / 2, thickness / 2])
b.add_box_visual(Pose([0, 0, thickness / 2]), [size[0] / 2, size[1] / 2, thickness / 2],
[0.1, 0.8, 1])
b.add_box_shape(Pose([0, size[1] / 2, size[2] / 2]),
[size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2])
b.add_box_visual(Pose([0, size[1] / 2, size[2] / 2]),
[size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2], [0.1, 0.8, 1])
b.add_box_shape(Pose([0, -size[1] / 2, size[2] / 2]),
[size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2])
b.add_box_visual(Pose([0, -size[1] / 2, size[2] / 2]),
[size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2], [0.1, 0.8, 1])
b.add_box_shape(Pose([size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2])
b.add_box_visual(Pose([size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2],
[0.1, 0.8, 1])
b.add_box_shape(Pose([-size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2])
b.add_box_visual(Pose([-size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2],
[0.1, 0.8, 1])
self.bin = b.build(True, name="bin")
self.bin.set_pose(Pose(p, axangle2quat([0, 0, 1], r)))
forward /= np.linalg.norm(forward)
out_info['gripper_forward_direction_world'] = forward.tolist()
forward_cam = np.linalg.inv(cam.get_metadata()['mat44'][:3, :3]) @ forward
out_info['gripper_forward_direction_camera'] = forward_cam.tolist()
rotmat = np.eye(4).astype(np.float32)
rotmat[:3, 0] = forward
rotmat[:3, 1] = left
rotmat[:3, 2] = up
final_dist = 0.1
if primact_type == 'pushing-left' or primact_type == 'pushing-up':
final_dist = 0.11
final_rotmat = np.array(rotmat, dtype=np.float32)
final_rotmat[:3, 3] = position_world - action_direction_world * final_dist
final_pose = Pose().from_transformation_matrix(final_rotmat)
out_info['target_rotmat_world'] = final_rotmat.tolist()
start_rotmat = np.array(rotmat, dtype=np.float32)
start_rotmat[:3, 3] = position_world - action_direction_world * 0.15
start_pose = Pose().from_transformation_matrix(start_rotmat)
out_info['start_rotmat_world'] = start_rotmat.tolist()
action_direction = None
if 'left' in primact_type:
action_direction = forward
elif 'up' in primact_type:
action_direction = left
if action_direction is not None:
end_rotmat = np.array(rotmat, dtype=np.float32)
def check_inside_bin(self, point):
size = self.bin_size
[x, y, z] = self.bin.pose.inv().transform(Pose(point)).p
return -size[0] / 2 < x < size[0] / 2 and \
-size[1] / 2 < y < size[1] / 2 and \
0 < z < size[2]
def act(self, env: FinalEnv, current_timestep: int):
"""called at each (actionable) time step to set robot actions. return False to
indicate that the agent decides to move on to the next environment.
Returning False early could mean a lower success rate (correctly placed
boxes / total boxes), but it can also save you some time, so given a
fixed total time budget, you may be able to place more boxes.
"""
# robot_left, robot_right, camera_front, camera_left, camera_right, camera_top = env.get_agents()
robot_left, robot_right, c1, c2, c3, c4 = env.get_agents()
pf_left = f = robot_left.get_compute_functions()['passive_force'](True, True, False)
pf_right = f = robot_right.get_compute_functions()['passive_force'](True, True, False)
if self.phase == 0:
self.counter += 1
t1 = [2, 1, 0, -1.5, -1, 1, -2.7]
t2 = [-2, 1, 0, -1.5, 1, 1, -2]
robot_left.set_action(t1, [0] * 7, pf_left)
robot_right.set_action(t2, [0] * 7, pf_right)
if np.allclose(robot_left.get_observation()[0], t1, 0.05, 0.05) and np.allclose(
robot_right.get_observation()[0], t2, 0.05, 0.05):
self.phase = 1
self.counter = 0
self.selected_x = None
self.spade_width, self.spade_length = self.get_spade(c4)
print(self.spade_width, self.spade_length)
if (self.counter > 8000/5):
self.counter = 0
return False
if self.phase == 1:
# print(11111)
self.counter += 1
if (self.counter == 1):
self.selected_x, self.selected_z = self.pick_box(c4)
print(self.selected_x,self.selected_z)
if self.selected_x is None:
self.counter = 0
self.phase = 0
return False
# change 0.67 to 0.69
target_pose_left = Pose([-0.3, 0.1, 0.55], euler2quat(np.pi, -np.pi / 3, -np.pi/2 ))
self.diff_drive(robot_left, 9, target_pose_left)
target_pose_right = Pose([-0.3, 0.1, 0.55], euler2quat(np.pi, -np.pi / 3, np.pi/2))
self.diff_drive(robot_right, 9, target_pose_right)
if self.counter == 2000 / 5:
self.phase = 2
self.counter = 0
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
p[0] = 1
self.pose_left = Pose(p, q)
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
p[0] = 1
self.pose_right = Pose(p, q)
if self.phase == 2:
# print(22222)
self.counter += 1
self.diff_drive(robot_left, 9, self.pose_left)
self.diff_drive(robot_right, 9, self.pose_right)
if self.counter == 600 / 5:
self.phase = 3
self.counter = 0
if self.phase == 3:
self.counter += 1
if (self.counter < 400/5):
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
p[0] = -3
self.diff_drive(robot_left, 9, Pose(p, q))
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
p[0] = -3
self.diff_drive(robot_right, 9, Pose(p, q))
else:
t1 = [2, 1, 0, -1.5, -1, 1, -2]
t2 = [-2, 1, 0, -1.5, 1, 1, -2]
robot_left.set_action(t1, [0] * 7, pf_left)
robot_right.set_action(t2, [0] * 7, pf_right)
if np.allclose(robot_left.get_observation()[0], t1, 0.05, 0.05) and np.allclose(
robot_right.get_observation()[0], t2, 0.05, 0.05):
self.phase = 4
self.counter = 0
self.selected_x = None
self.spade_width, self.spade_length = self.get_spade(c4)
print(self.spade_width, self.spade_length)
if (self.counter > 8000/5):
self.counter = 0
return False
if self.phase == 4:
# print(11111)
self.counter += 1
if (self.counter == 1):
self.selected_x, self.selected_z = self.pick_box(c4)
print(self.selected_x,self.selected_z)
if self.selected_x is None:
self.counter = 0
self.phase = 0
return False
# change 0.67 to 0.69
target_pose_left = Pose([self.selected_x, 0.6, self.selected_z + 0.65], euler2quat(np.pi, -np.pi / 3, -np.pi / 2))
self.diff_drive(robot_left, 9, target_pose_left)
target_pose_right = Pose([self.selected_x, -0.6, self.selected_z + 0.55], euler2quat(np.pi, -np.pi / 3, np.pi / 2))
self.diff_drive(robot_right, 9, target_pose_right)
if self.counter == 1500 / 5:
self.phase = 5
self.counter = 0
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = self.spade_length/2
self.pose_left = Pose(p, q)
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] = - self.spade_length / 2
self.pose_right = Pose(p, q)
if self.phase == 5:
# print(22222)
self.counter += 1
self.diff_drive(robot_left, 9, self.pose_left)
self.diff_drive(robot_right, 9, self.pose_right)
if self.counter == 2500 / 5:
self.phase = 6
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
# p[2] += 0.1
self.pose_right = Pose(p, q)
# pose = robot_left.get_observation()[2][9]
# p, q = pose.p, pose.q
# p[1] = -0.3
# p[2] += 0.3
# q = euler2quat(np.pi, -np.pi / 8, -np.pi / 2)
# self.pose_left = Pose(p, q)
self.counter = 0
if self.phase == 6:
# print(33333)
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
# p[1] -= 0.1
p[2] += 0.1
q = euler2quat(np.pi, -np.pi / 4, -np.pi /2)
self.pose_left = Pose(p, q)
self.counter += 1
self.diff_drive(robot_left, 9, self.pose_left)
if self.counter == 200 / 5:
self.phase = 7
self.counter = 0
if self.phase == 7:
# print(self.counter)
# self.diff_drive(robot_right, 9, self.pose_right)
self.counter += 1
if (self.counter < 200 / 5):
pose = robot_left.get_observation()[2][9]
p, q = pose.p, pose.q
p[1] += 5
p[2] += 5
# q = euler2quat(np.pi, 0, -np.pi / 4)
# q = euler2quat(np.pi, -np.pi / 4, -np.pi /2)
self.diff_drive(robot_left, 9, Pose(p, q))
elif (self.counter < 300 / 5):
pose = robot_right.get_observation()[2][9]
p, q = pose.p, pose.q
q = euler2quat(0,0 ,np.pi/4)
self.diff_drive(robot_right, 9, Pose(p, q))
elif (self.counter < 6500 / 5):
p = [self.top_view[0], self.top_view[1] + self.spade_length /3, 2.0*self.front_view[2]]
q = euler2quat(0, -np.pi / 1.5, 0)
self.diff_drive(robot_right, 9, Pose(p, q))
# if self.counter == 500/5:
# cur_box = self.check_box(c2)
# print(self.box_num, cur_box)
# if (cur_box >= self.box_num):
# self.phase = 0
# return False
elif (self.counter < 9000 / 5):
pose = robot_right.get_observation()[2][9]
p = pose.p
q = euler2quat(0, -np.pi / 1.5, 0)
self.diff_drive(robot_right, 9, Pose(p, q))
else:
self.phase = 0
self.counter = 0
return False
def after_substep(self, env) -> None:
assert self._initialized, "task not initialized"
if self._box_valid_tumble is False or self._suceeded or not self._tracking:
self._tracking = False
return
box_pose = self._box.observation['pose']
# calculate how much "flipped"
rot_mat = quat2mat(box_pose.q)
box_up = rot_mat[:3, 2]
box_up /= np.linalg.norm(box_up)
# check height
if box_pose.p[2] > self._box_height_thresh:
self._suceeded = False
self._box_valid_tumble = False
self._failed_reason = FAIL_REASON_TOO_HIGH
return
tiled = np.arccos(box_up[2]) > TILT_THRESHOLD
theta_x, theta_y = np.pi / 2, np.pi / 2
if tiled:
theta_x, theta_y = np.arccos(self._last_basis @ box_up)
else:
self._last_basis = rot_mat[:3, :2]
self._last_basis[2, :] = 0
self._last_basis /= np.linalg.norm(self._last_basis, axis=0)
self._last_basis = self._last_basis.T
tumble_x = theta_x < np.pi / 2 - TILT_THRESHOLD
tumble_y = theta_y < np.pi / 2 - TILT_THRESHOLD
# check tumble
self._box_valid_tumble = not (tumble_x and tumble_y)
if self._box_valid_tumble is False:
self._suceeded = False
self._failed_reason = FAIL_REASON_DIAG_FLIP
return
# check edge center
if tumble_x or tumble_y:
center2pt1 = np.array([0, 0, -self._box.box_size])
center2pt2 = center2pt1.copy()
if tumble_x:
center2pt1[0] = np.sign(theta_x) * self._box.box_size
center2pt1[1] = self._box.box_size/2
center2pt2[0] = np.sign(theta_x) * self._box.box_size
center2pt2[1] = -self._box.box_size/2
else:
center2pt1[1] = np.sign(theta_y) * self._box.box_size
center2pt1[0] = self._box.box_size/2
center2pt2[1] = np.sign(theta_y) * self._box.box_size
center2pt2[0] = -self._box.box_size/2
center2pt1_pose = Pose(center2pt1)
edge_pt1 = (box_pose * center2pt1_pose).p
center2pt2_pose = Pose(center2pt2)
edge_pt2 = (box_pose * center2pt2_pose).p
if self._last_edge_pt1 is not None and self._last_edge_pt2 is not None:
dist1 = np.linalg.norm(self._last_edge_pt1 - edge_pt1)
dist2 = np.linalg.norm(self._last_edge_pt2 - edge_pt2)
if dist1 > BOX_EDGE_DISP_THRESHOLD or dist2 > BOX_EDGE_DISP_THRESHOLD:
self._box_valid_tumble = False
self._failed_reason = FAIL_REASON_EDGE_MOVED
return
else:
self._last_edge_pt1 = edge_pt1
self._last_edge_pt2 = edge_pt2
else:
self._last_edge_pt1 = None
self._last_edge_pt2 = None
# check succeeded
if tumble_x ^ tumble_y and \
(theta_x < np.pi/2 - FULL_TUMBLE_THRESHOLD or theta_y < np.pi/2 - FULL_TUMBLE_THRESHOLD):
self._suceeded = True
def build_table(self):
b = self.scene.create_actor_builder()
b.add_multiple_convex_shapes_from_file('./assets/bigboard.obj', Pose([-0.3, 0, 0]))
b.add_visual_from_file('./assets/bigboard.obj', Pose([-0.3, 0, 0]))
self.table = b.build_static(name="table")
def setup_target(self, position, size):
self.target = (position, size)
builder = self.scene.create_actor_builder()
builder.add_box_visual(size=[size] * 3, color=[0, 1, 1])
indicator = builder.build_static('goal')
indicator.set_pose(Pose([position[0], position[1], -size + 0.001]))