def robot_basic_control_demo(use_internal_pd, use_external_pid): 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 / 2000) 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 = True robot = loader.load("assets/robot/jaco2.urdf") arm_init_qpos = [4.71, 2.84, 0, 0.75, 4.62, 4.48, 4.88] arm_zero_qpos = [0, 3.14, 0, 3.14, 0, 3.14, 0] gripper_init_qpos = [0, 0, 0, 0, 0, 0] target_pos = np.array(arm_init_qpos + gripper_init_qpos) zero_qpos = arm_zero_qpos + gripper_init_qpos robot.set_qpos(zero_qpos) pids = [] pid_parameters = [(40, 5, 2), (40, 5.0, 2), (40, 5.0, 2), (20, 5.0, 2), (5, 0.8, 2), (5, 0.8, 2), (5, 0.8, 0.4), (0.1, 0, 0.02), (0.1, 0, 0.02), (0.1, 0, 0.02), (0.1, 0, 0.02), (0.1, 0, 0.02), (0.1, 0, 0.02)] active_joints = [joint for joint in robot.get_joints() if joint.get_dof() > 0] for i, joint in enumerate(active_joints): if use_internal_pd: joint.set_drive_property(stiffness=5, damping=5) if use_external_pid: pids.append(SimplePID(pid_parameters[i][0], pid_parameters[i][1], pid_parameters[i][2])) robot.set_drive_target(target_pos) steps = 0 renderer_controller.show_window() while not renderer_controller.should_quit: scene0.update_render() for i in range(4): qf = robot.compute_passive_force(gravity=True, coriolis_and_centrifugal=True, external=True) if use_external_pid: pid_qf = pid_forward(pids, target_pos, robot.get_qpos(), scene0.get_timestep()) qf += pid_qf robot.set_qf(qf) scene0.step() steps += 1 renderer_controller.render() scene0 = None
def viewer_setup(self, width=640, height=480) -> sapien.OptifuserController: self.sim.set_ambient_light([.4, .4, .4]) self.sim.set_shadow_light([1, -1, -1], [.5, .5, .5]) self.sim.add_point_light([2, 2, 2], [1, 1, 1]) self.sim.add_point_light([2, -2, 2], [1, 1, 1]) self.sim.add_point_light([-2, 0, 2], [1, 1, 1]) controller = sapien.OptifuserController(self._renderer) controller.set_current_scene(self.sim) controller.set_camera_position(0, 1, 10) controller.set_camera_rotation(0, -1.5) controller.show_window() return controller
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 __init__(self, timestep=1 / 500, frame_skip=5): self.global_total_timesteps = 0 self.global_max_steps = 1000000 # the total time budget is 2000 seconds (in actual testing it will be greater) self.local_total_timesteps = 0 self.local_max_steps = 100000 # each run needs to end after 200 seconds self.total_box_genreated = 0 self.total_box_picked = 0 self.frame_skip = frame_skip self.window = False self.engine = sapien.Engine(0, 0.001, 0.005) self.renderer = sapien.OptifuserRenderer(config=render_config) self.renderer.enable_global_axes(False) self.engine.set_renderer(self.renderer) self.renderer_controller = sapien.OptifuserController(self.renderer) # self.renderer_controller.set_camera_position(-2, 0, 1) self.renderer_controller.set_camera_position(-1.3, 0, 4.1) # self.renderer_controller.set_camera_rotation(0, -0.5) self.renderer_controller.set_camera_rotation(0, -1.4) scene_config = SceneConfig() scene_config.gravity = [0, 0, -9.81] self.scene = self.engine.create_scene(config=scene_config) self.renderer_controller.set_current_scene(self.scene) self.scene.set_timestep(timestep) self.scene.set_shadow_light([0.2, -0.2, -1], [1, 1, 1]) self.scene.set_ambient_light([0.3, 0.3, 0.3]) self.build_room() self.build_table() self.build_camreas() self.left_robot: Robot = None self.right_robot: Robot = None loader = self.scene.create_urdf_loader() self.robot_builder = loader.load_file_as_articulation_builder('./assets/robot/panda_spade.urdf') self.bin = None self.boxes: List[sapien.Actor] = [] for i in range(10): s = np.random.random() * 0.01 + 0.01 d = self.create_dice([s, s, s]) self.boxes.append(d)
def __init__(self, timestep: float = 1 / 60, frame_skip: int = 1, visual: bool = True, cam_pos=None, cam_rot=None): if cam_rot is None: cam_rot = [-np.pi, -np.pi/9] if cam_pos is None: cam_pos = [2, 0, 0] # initialize sapien self._sim = sapien.Engine() if visual: self._renderer = sapien.OptifuserRenderer() self._sim.set_renderer(self._renderer) self._render_controller = sapien.OptifuserController(self._renderer) self._render_controller.set_camera_position(*cam_pos) self._render_controller.set_camera_rotation(*cam_rot) sceneConfig = sapien.SceneConfig() sceneConfig.sleep_threshold = 0.00002 self._scene = self._sim.create_scene(config=sceneConfig) self._scene.add_ground(-1) self._scene.set_timestep(timestep) if visual: self._scene.set_ambient_light([0.5, 0.5, 0.5]) self._scene.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5]) self._render_controller.set_current_scene(self._scene) # set up variables self._timestep = timestep self._frame_skip = frame_skip self._step = 0 self._tasks: [Task] = [] self._agents: Dict[str, Agent] = {} self._shown_window = False # set up basic environment self._set_up_basic_env() for _ in range(100): self._scene.step() self._render_controller.render()
import sapien.core as sapien from sapien.core import Pose sim = sapien.Engine() 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:
def __init__(self, flog=None, show_gui=True, render_rate=20, timestep=1/500, \ object_position_offset=0.0, succ_ratio=0.1): self.current_step = 0 self.flog = flog self.show_gui = show_gui self.render_rate = render_rate self.timestep = timestep self.succ_ratio = succ_ratio self.object_position_offset = object_position_offset # engine and renderer self.engine = sapien.Engine(0, 0.001, 0.005) render_config = OptifuserConfig() render_config.shadow_map_size = 8192 render_config.shadow_frustum_size = 10 render_config.use_shadow = False render_config.use_ao = True self.renderer = sapien.OptifuserRenderer(config=render_config) self.renderer.enable_global_axes(False) self.engine.set_renderer(self.renderer) # GUI self.window = False if show_gui: self.renderer_controller = sapien.OptifuserController( self.renderer) self.renderer_controller.set_camera_position( -3.0 + object_position_offset, 1.0, 3.0) self.renderer_controller.set_camera_rotation(-0.4, -0.8) # scene scene_config = SceneConfig() scene_config.gravity = [0, 0, -9.81] scene_config.solver_iterations = 20 scene_config.enable_pcm = False scene_config.sleep_threshold = 0.0 self.scene = self.engine.create_scene(config=scene_config) if show_gui: self.renderer_controller.set_current_scene(self.scene) self.scene.set_timestep(timestep) # add lights self.scene.set_ambient_light([0.5, 0.5, 0.5]) self.scene.set_shadow_light([0, 1, -1], [0.5, 0.5, 0.5]) self.scene.add_point_light([1 + object_position_offset, 2, 2], [1, 1, 1]) self.scene.add_point_light([1 + object_position_offset, -2, 2], [1, 1, 1]) self.scene.add_point_light([-1 + object_position_offset, 0, 1], [1, 1, 1]) # default Nones self.object = None self.object_target_joint = None # check contact self.check_contact = False
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 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")