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()
self._engine.set_renderer(self._renderer)
self.sim = self._engine.create_scene()
self.sim.set_timestep(timestep)
self.viewer = 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)
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 __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 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
import sapien.core as sapien
engine = sapien.Engine()
renderer = sapien.OptifuserRenderer()
engine.set_renderer(renderer)
scene0 = engine.create_scene(gravity=[0, 0, -9.81])
scene0.set_timestep(1 / 240)
renderer_controller = sapien.OptifuserController(renderer)
renderer_controller.set_current_scene(scene0)
renderer_controller.show_window()
renderer_controller.set_camera_position(-4, 0, 2)
renderer_controller.set_camera_rotation(0, -0.5)
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()
articulated_object = loader.load("assets/179/mobility.urdf")
while not renderer_controller.should_quit:
scene0.update_render()
scene0.step()
renderer_controller.render()
scene0 = None
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")
import sapien.core as pysapien
import sapien
import numpy as np
sim = pysapien.Engine()
renderer = pysapien.OptifuserRenderer()
sim.set_renderer(renderer)
render_controller = pysapien.OptifuserController(renderer)
render_controller.show_window()
s0 = sim.create_scene(gravity=[0, 0, -9.8])
s0.add_ground(-1)
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])
loader: pysapien.URDFLoader = s0.create_urdf_loader()
loader.fix_root_link = 1
urdf = sapien.asset.download_partnet_mobility(
45092,
"eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJlbWFpbCI6InkxcWluQGVuZy51Y3NkLmVkdSIsImlwIjoiNjYuNzUuMjQwLjYwIiwicHJpdmlsZWdlIjoxMCwiaWF0IjoxNTg2NzM0NDQxLCJleHAiOjE2MTgyNzA0NDF9.5Q_eJyzP_4Jf7wxTa068sB8q666AJgcFE2ojf3UsHkc"
)
robot = loader.load(urdf)
render_controller.set_camera_position(-5, 0, 0)
render_controller.set_current_scene(s0)
steps = 0
while not render_controller.should_quit:
