def __init__(self, timestep: float):
"""Class for homework2
Args:
timestep: timestep to balance the precision and the speed of physical simulation
"""
StackingEnv.__init__(self, timestep)
self.near, self.far = 0.1, 100
self.camera_link = self.scene.create_actor_builder().build(
is_kinematic=True)
self.gl_pose = sapien.Pose([0, 0, 0], [0.5, -0.5, 0.5, -0.5])
self.camera_link.set_pose(
sapien.Pose([1.2, 0.0, 0.8], [0, -0.258819, 0, 0.9659258]))
self.camera = self.scene.add_mounted_camera('fixed_camera',
self.camera_link,
sapien.Pose(), 1920, 1080,
np.deg2rad(50),
np.deg2rad(50), self.near,
self.far)
self.arm_joints = [
joint for joint in self.robot.get_joints() if joint.get_dof() > 0
and not joint.get_name().startswith("panda_finger")
]
self.set_joint_group_property(self.arm_joints, 1000, 400)
assert len(self.arm_joints) == self.robot.dof - 2
self.set_joint_group_property(self.gripper_joints, 200, 60)
self.step()
self.robot.set_drive_target(self.robot.get_qpos())
def __init__(self, timestep: float):
"""Class for homework1
Args:
timestep: timestep to balance the precision and the speed of physical simulation
"""
StackingEnv.__init__(self, timestep)
self.near, self.far = 0.1, 100
self.camera_link = self.scene.create_actor_builder().build(is_kinematic=True)
self.gl_pose = sapien.Pose([0, 0, 0], [0.5, -0.5, 0.5, -0.5])
self.camera_link.set_pose(sapien.Pose([1.2, 0.0, 0.8], [0, -0.258819, 0, 0.9659258]))
self.camera = self.scene.add_mounted_camera('fixed_camera', self.camera_link,
sapien.Pose(), 1920, 1080,
np.deg2rad(50), np.deg2rad(50), self.near, self.far)
def diff_drive(ball, hand, arm_controller, local_ball, scale):
target_pos = ball.get_pose().p
hand_pos: sapien.Pose = hand.get_pose()
current_pos = hand_pos.to_transformation_matrix()[:3,
2] * 0.11 + hand_pos.p
local_ball.set_pose(sapien.Pose(current_pos))
diff = target_pos - current_pos
if np.linalg.norm(diff) < 0.02:
return True
arm_controller.move_cartesian(diff * scale, sr.MoveType.WORLD_TRANSLATE)
def _load_robot(self, urdf_path: str, material: sapien.PxMaterial) -> None:
# By default, the robot will loaded with balanced passive force
self.loader.fix_base = True
self.robot: sapien.Articulation = self.loader.load(urdf_path, material)
self.robot.set_root_pose(sapien.Pose([0, 0, 0], [1, 0, 0, 0]))
# Link mapping, remember to set the self._base_link_name if your robot base is not that name
self._q_names = [
j.name for j in self.robot.get_joints() if j.get_dof()
]
self._base_link_name = "base_link"
# Set mapping and other staff for the camera loaded with robot urdf
self._init_camera_cache()
def robot_basic_control_demo(fix_robot_root, balance_passive_force,
add_joint_damping):
sim = sapien.Engine()
renderer = sapien.OptifuserRenderer()
renderer.enable_global_axes(False)
sim.set_renderer(renderer)
renderer_controller = sapien.OptifuserController(renderer)
renderer_controller.set_camera_position(-2, 0, 1)
renderer_controller.set_camera_rotation(0, -0.5)
scene0 = sim.create_scene(gravity=[0, 0, -9.81])
renderer_controller.set_current_scene(scene0)
scene0.add_ground(altitude=0)
scene0.set_timestep(1 / 240)
scene0.set_ambient_light([0.5, 0.5, 0.5])
scene0.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])
loader = scene0.create_urdf_loader()
loader.fix_root_link = fix_robot_root
robot = loader.load("assets/robot/jaco2.urdf")
robot.set_root_pose(sapien.Pose([0, 0, 0], [1, 0, 0, 0]))
arm_init_qpos = [4.71, 2.84, 0, 0.75, 4.62, 4.48, 4.88]
gripper_init_qpos = [0, 0, 0, 0, 0, 0]
init_qpos = arm_init_qpos + gripper_init_qpos
robot.set_qpos(init_qpos)
if add_joint_damping:
for joint in robot.get_joints():
joint.set_drive_property(stiffness=0, damping=10)
steps = 0
renderer_controller.show_window()
while not renderer_controller.should_quit:
scene0.update_render()
for i in range(4):
if balance_passive_force:
qf = robot.compute_passive_force(gravity=True,
coriolisAndCentrifugal=True,
external=False)
robot.set_qf(qf)
scene0.step()
steps += 1
renderer_controller.render()
scene0 = None
def add_camera(self,
name: str,
camera_pose: Union[np.ndarray, sapien.Pose],
width: int,
height: int,
fov=1.1,
near=0.01,
far=100) -> None:
"""
Add custom mounted camera to the scene. These camera have same property as the urdf-defined camera
:param name: Name of the camera, used for get camera name and may be used for namespace of topic if ROS enabled
:param camera_pose: (4, 4) transformation matrix to define the pose of camera. Camera point forward along
positive x-axis, the y-axis and z-axis accounts for the width and height of the image captured by camera
:param width: The width of the camera, e.g. 1920 in the (1920, 1080) hd image
:param height: The height of the camera, e.g. 1080 in the (1920, 1080) hd image
:param fov: Field of view angle in arc. Note the fov is the full angle of view, not half angle. Currently,
we do not support differernt fov for x and y axis and thus we can only render square pixel
:param near: Minimum distance camera can observe, it will influence all texture channel
:param far: Maximum distance camera can observe, it will influence all texture channel
"""
actor = self.builder.build(False, True, "{}".format(name), True)
if isinstance(camera_pose, np.ndarray):
assert camera_pose.shape == (
4, 4), "Camera pose matrix must be (4, 4)"
pose = mat2transform(camera_pose)
elif isinstance(camera_pose, sapien.Pose):
pose = camera_pose
camera_pose = transform2mat(pose)
else:
raise RuntimeError("Unknown format of camera pose: {}".format(
type(camera_pose)))
camera = self.sim.add_mounted_camera(
name, actor, sapien.Pose([0, 0, 0], [1, 0, 0, 0]), width, height,
fov, fov, near, far)
actor.set_global_pose(pose)
self.cam_list.append(camera)
self.mount_actor_list.append(actor)
self.camera_name_list.append(name)
self.__build_camera_mapping(height, width, camera.get_camera_matrix())
self.depth_lambda_list.append(
lambda depth: 1 / (depth * (1 / far - 1 / near) + 1 / near))
self.camera_frame_id.append("/base_link")
self.camera_pose.append(
(camera_pose @ _CAMERA_TO_LINK).astype(np.float32))
# Build OpenGL camera mapping
width_points = np.repeat(np.linspace(1 / (2 * width),
1 - 1 / (2 * width),
width).reshape([1, -1]),
height,
axis=0)
height_points = np.repeat(np.linspace(1 / (2 * height),
1 - 1 / (2 * height),
height)[::-1].reshape([-1, 1]),
width,
axis=1)
points = np.stack([width_points, height_points], 2) * 2 - 1
homo_padding = np.ones_like(width_points) * 2 - 1
self.__gl_camera_mapping.append((points, homo_padding[:, :,
np.newaxis]))
self._gl_projection_inv.append(
np.linalg.inv(camera.get_projection_mat()))
def main():
engine = sapien.Engine()
renderer = sapien.OptifuserRenderer()
engine.set_renderer(renderer)
controller = sapien.OptifuserController(renderer)
scene: sapien.Scene = engine.create_scene()
scene.set_timestep(1 / 200)
scene.add_ground(0)
controller.set_current_scene(scene)
controller.set_camera_position(-0.5, 2, 0.5)
controller.set_camera_rotation(-1, 0)
scene.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])
scene.set_ambient_light((0.5, 0.5, 0.5))
# Gif recorder
camera_mount_actor = scene.create_actor_builder().build(is_kinematic=True)
camera_mount_actor.set_pose(
sapien.Pose([-0.5, 1.5, 0.5], [0.8775826, 0, 0, -0.4794255]))
camera = scene.add_mounted_camera('first_camera', camera_mount_actor,
sapien.Pose(), 640, 480, np.deg2rad(35),
np.deg2rad(35), 0.1, 100)
l = scene.create_urdf_loader()
with open("/home/sim/project/sapien/example/assets/robot/xarm6.urdf") as f:
data = f.read()
data = substitute_path(
data, "/home/sim/project/sapien/example/assets/robot")
# robot0 = l.load_from_string(data, "")
# Manager
scene_manager = ros2.SceneManager(scene, "scene1")
loader = scene_manager.create_robot_loader()
loader.fix_root_link = True
robot_descriptor = ros2.RobotDescriptor(
"sapien_resources", "xarm6_description/urdf/xarm6.urdf",
"xarm6_moveit_config/config/xarm6.srdf", True)
robot, manager = loader.load_robot_and_manager(robot_descriptor, "my_xarm")
robot.set_root_pose(sapien.Pose())
robot.set_qpos(robot.dof * [0])
robot.set_qf(robot.dof * [1])
loader = scene_manager.create_robot_loader()
loader.fix_root_link = True
# Load second robot using
robot_descriptor2 = ros2.RobotDescriptor(True,
"assets_local/robot/panda.urdf",
"")
robot2, manager2 = loader.load_robot_and_manager(robot_descriptor2,
"panda")
robot2.set_root_pose(sapien.Pose([2, 0, 0]))
manager.set_drive_property(1000, 50, 5000, np.arange(6))
manager.set_drive_property(0, 50, 100, np.arange(6, 12))
# Controller
arm_latency = 0.1
gripper_joints = [
"drive_joint", "left_finger_joint", "left_inner_knuckle_joint",
"right_outer_knuckle_joint", "right_finger_joint",
"right_inner_knuckle_joint"
]
manager.create_joint_publisher(20)
gripper_controller = manager.build_joint_velocity_controller(
gripper_joints, "gripper_joint_velocity", 0)
arm_controller = manager.build_cartesian_velocity_controller(
"arm", "arm_cartesian_velocity", arm_latency)
# Start
controller.show_window()
scene_manager.start()
step = 0
print("====================================================")
while not controller.should_quit:
scene.update_render()
scene.step()
controller.render()
manager2.balance_passive_force()
step += 1
gripper_controller.move_joint(np.ones(6) * 5, True)
def rpy2transform(rpy: np.ndarray) -> sapien.Pose:
assert rpy.shape == (3, )
pose = sapien.Pose(np.zeros(3), transforms3d.euler.euler2quat(rpy))
return pose
def rot2transform(rot: np.ndarray) -> sapien.Pose:
assert rot.shape == (3, 3)
pose = sapien.Pose(np.zeros(3), transforms3d.quaternions.mat2quat(rot))
return pose
def mat2transform(mat: np.ndarray) -> sapien.Pose:
assert mat.shape == (4, 4)
pose = sapien.Pose(mat[:3, 3],
transforms3d.quaternions.mat2quat(mat[:3, :3]))
return pose
def main(twist_example, screw_example):
engine = sapien.Engine()
renderer = sapien.OptifuserRenderer()
renderer.enable_global_axes(True)
engine.set_renderer(renderer)
controller = sapien.OptifuserController(renderer)
scene: sapien.Scene = engine.create_scene()
scene.set_timestep(1 / 200)
scene.add_ground(0)
controller.set_current_scene(scene)
controller.set_camera_position(-0.8, 0.875, 0.65)
controller.set_camera_rotation(-1, 0)
scene.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])
scene.set_ambient_light((0.5, 0.5, 0.5))
# Gif recorder
degree = 1.0 * np.pi / 180
recorder = GifRecorder()
camera_mount_actor = scene.create_actor_builder().build(is_kinematic=True)
camera_mount_actor.set_pose(
sapien.Pose([-0.6, 0.65, 0.95], transforms3d.euler.euler2quat(0, 20 * degree, -40 * degree, "sxyz")))
camera = scene.add_mounted_camera('first_camera', camera_mount_actor, sapien.Pose(), 1920, 1080, np.deg2rad(35),
np.deg2rad(35), 0.1, 100)
scene_manager = sr.SceneManager(scene, "example_scene")
loader = scene_manager.create_robot_loader()
loader.fix_root_link = True
descriptor = sr.RobotDescriptor(is_path=True, urdf="../../assets_local/robot/panda.urdf", srdf="")
robot, manager = loader.load_robot_and_manager(descriptor, "panda")
robot.set_root_pose(sapien.Pose([0, 0, 0], [1, 0, 0, 0]))
robot.set_qpos([0, 0, 0, -1.5, 0, 1.5, 0.7, 0.4, 0.4])
robot.set_drive_target([0, 0, 0, -1.5, 0, 1.5, 0.7, 0.4, 0.4])
manager.set_drive_property(3000, 500, 50000, np.arange(9))
manager.create_joint_publisher(20)
arm_controller = manager.build_cartesian_velocity_controller("panda_arm", "arm_cartesian_velocity", 0)
gripper_controller = manager.build_joint_velocity_controller(["panda_finger_joint1", "panda_finger_joint2"],
"panda_gripper")
actor_builder = scene.create_actor_builder()
actor_builder.add_visual_from_file("../../assets_local/small_arrow.dae")
# arrow = actor_builder.build_static()
# arrow.set_pose(sapien.Pose([0.8, -0.25, 0.25]))
# Start
controller.show_window()
scene_manager.start()
step = 0
import time
time.sleep(2.0)
while not controller.should_quit:
scene.step()
scene.update_render()
controller.render()
step += 1
if step % 10 == 0:
camera.take_picture()
recorder.record(camera.get_color_rgba())
if step > 1000:
manager.balance_passive_force()
arm_controller.move_twist([0.0, -0.1, -0.1, -0.4, 0.0, 0.0], sr.MoveType.SPATIAL_TWIST)
def main():
engine = sapien.Engine()
renderer = sapien.OptifuserRenderer()
engine.set_renderer(renderer)
controller = sapien.OptifuserController(renderer)
scene: sapien.Scene = engine.create_scene(gravity=[0, 0, 0])
scene.set_timestep(1 / 200)
scene.add_ground(0)
controller.set_current_scene(scene)
controller.set_camera_position(-0.5, 2, 0.5)
controller.set_camera_rotation(-1, 0)
scene.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])
scene.set_ambient_light((0.5, 0.5, 0.5))
# Gif recorder
recorder = GifRecorder()
camera_mount_actor = scene.create_actor_builder().build(is_kinematic=True)
camera_mount_actor.set_pose(
sapien.Pose([-0.5, 1.5, 0.5], [0.8775826, 0, 0, -0.4794255]))
camera = scene.add_mounted_camera('first_camera', camera_mount_actor,
sapien.Pose(), 640, 480, np.deg2rad(35),
np.deg2rad(35), 0.1, 100)
loader = scene.create_urdf_loader()
loader.fix_root_link = True
loader.scale = 1
robot = loader.load("example/assets/robot/xarm6.urdf")
robot.set_root_pose(sapien.Pose([0, 0, 0.1], [1, 0, 0, 0]))
# Manager
scene_manager = pysapien_ros2.SceneManager(scene, "scene1")
robot_manager: pysapien_ros2.RobotManager = scene_manager.build_robot_manager(
robot, "xarm6")
robot_manager.set_drive_property(1000, 50, 5000, np.arange(6))
robot_manager.set_drive_property(0, 50, 100, np.arange(6, 12))
# Controller
arm_latency = 0.1
gripper_joints = [
"drive_joint", "left_finger_joint", "left_inner_knuckle_joint",
"right_outer_knuckle_joint", "right_finger_joint",
"right_inner_knuckle_joint"
]
robot_manager.create_joint_publisher(20)
gripper_controller = robot_manager.build_joint_velocity_controller(
gripper_joints, "gripper_joint_velocity", 0)
arm_controller = robot_manager.build_cartesian_velocity_controller(
"arm", "arm_cartesian_velocity", arm_latency)
# Start
controller.show_window()
scene_manager.start()
step = 0
# Set moving ball
builder: sapien.ActorBuilder = scene.create_actor_builder()
ball_pose = sapien.Pose([0.5, 0, 0.5])
builder.add_sphere_visual(radius=0.03)
ball: sapien.Actor = builder.build(is_kinematic=False, name="ball")
ball.set_pose(ball_pose)
angular_velocity = 0.7
filename = "gif/w_{}_latency_{}.gif".format(angular_velocity, arm_latency)
rotation_speed = angular_velocity * 9 / 10 / np.pi
ball_velocity = [
(0,
0.0 + np.sin(t / 180 * rotation_speed * np.pi) * 0.6 * rotation_speed,
-np.cos(t / 180 * rotation_speed * np.pi) * 0.6 * rotation_speed)
for t in range(360 * 100)
]
left_finger = robot.get_links()[-1]
right_finger = robot.get_links()[-4]
while not controller.should_quit:
scene.step()
if step % 5 == 0:
scene.update_render()
controller.render()
camera.take_picture()
recorder.record(camera.get_obj_segmentation() + 20)
robot_manager.balance_passive_force()
ball.set_velocity(ball_velocity[step % 36000])
step += 1
if diff_drive(ball, left_finger, right_finger, arm_controller, 4):
print("Success in step {}".format(step / 200))
recorder.save(filename)
controller.hide_window()
scene = None
return
if step > 10 * 200:
print("Fail!!")
recorder.save(filename)
controller.hide_window()
scene = None
return
gripper_controller.move_joint(np.ones(6) * 5, True)
def main():
engine = sapien.Engine()
renderer = sapien.OptifuserRenderer()
engine.set_renderer(renderer)
controller = sapien.OptifuserController(renderer)
scene: sapien.Scene = engine.create_scene(gravity=[0, 0, 0])
ground_material = sapien.PxrMaterial()
ground_color = np.array([202, 164, 114, 256]) / 256
ground_material.set_base_color(ground_color)
ground_material.specular = 0.5
scene.set_timestep(1 / 200)
scene.add_ground(0, render_material=ground_material)
controller.set_current_scene(scene)
controller.set_camera_position(-0.5, 2, 0.5)
controller.set_camera_rotation(-1, 0)
scene.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5])
scene.set_ambient_light((0.5, 0.5, 0.5))
# Gif recorder
recorder = GifRecorder()
camera_mount_actor = scene.create_actor_builder().build(is_kinematic=True)
camera_mount_actor.set_pose(
sapien.Pose([-0.5, 1.5, 0.5], [0.8775826, 0, 0, -0.4794255]))
camera = scene.add_mounted_camera('first_camera', camera_mount_actor,
sapien.Pose(), 640, 480, np.deg2rad(35),
np.deg2rad(35), 0.1, 100)
# Robot Loader
scene_manager = sr.SceneManager(scene, "scene1")
robot_descriptor = sr.RobotDescriptor(
True, "../../assets_local/robot/panda.urdf", "")
loader = scene_manager.create_robot_loader()
loader.fix_root_link = True
robot, robot_manager = loader.load_robot_and_manager(
robot_descriptor, "panda")
robot.set_qpos(np.array([0, 0, 0, -1.5, 0, 1.5, 0.7, 0.4, 0.4]))
robot.set_drive_target(robot.get_qpos())
robot_manager.set_drive_property(1000, 400, 10000, np.arange(7))
robot_manager.set_drive_property(50, 20, 500, np.arange(7, 9))
# Controller
gripper_joints = ["panda_finger_joint1", "panda_finger_joint2"]
arm_latency = 0.05
robot_manager.create_joint_publisher(20)
gripper_controller = robot_manager.build_joint_velocity_controller(
gripper_joints, "gripper_joint_velocity", 0)
arm_controller = robot_manager.build_cartesian_velocity_controller(
"panda_arm", "cartesian_velocity", arm_latency)
# Start
controller.show_window()
scene_manager.start()
step = 0
# Set moving ball
builder = scene.create_actor_builder()
ball_pose = sapien.Pose([0.5, 0, 0.3])
builder.add_sphere_visual(radius=0.04, color=[0.1, 0.1, 0.8])
ball = builder.build(is_kinematic=False, name="ball")
ball.set_pose(ball_pose)
# Set moving ball
builder = scene.create_actor_builder()
builder.add_sphere_visual(radius=0.02, color=[0.8, 0.1, 0.1])
local_ball = builder.build(is_kinematic=False, name="ball")
angular_velocity = 0.7
ball_center = np.array([0.5, 0, 0.3])
filename = "gif/w_{}_latency_{}.gif".format(angular_velocity, arm_latency)
rotation_speed = angular_velocity * 9 / 10 / np.pi
ball_velocity = [
(0,
0.0 + np.sin(t / 180 * rotation_speed * np.pi) * 0.6 * rotation_speed,
-np.cos(t / 180 * rotation_speed * np.pi) * 0.6 * rotation_speed)
for t in range(360 * 100)
]
panda_hand = robot.get_links()[-3]
while not controller.should_quit:
scene.step()
if step % 10 == 0:
scene.update_render()
controller.render()
camera.take_picture()
recorder.record(camera.get_color_rgba())
robot_manager.balance_passive_force()
ball.set_velocity(ball_velocity[step % 36000])
step += 1
if diff_drive(ball, panda_hand, arm_controller, local_ball, 1):
print("Success in step {}".format(step / 200))
return
#
if step > 10 * 200:
print("Fail!!")
return
gripper_controller.move_joint(np.ones(2) * -5, True)
controller.hide_window()
scene = None
recorder.save(filename)
print("Save")