def _load_controller_parameters(self):
# Init robot pose and controller
self.robot.set_pd(200, 60, 20, [0, 1, 2, 3, 4, 5])
self.robot.set_pd(1, 0.05, 1, [6, 7, 8])
self.init_qpos = [0, 0, 2, 0, 0, 0, 0, 0, 0]
self.robot.set_drive_qpos(self.init_qpos)
self.robot.set_qpos(self.init_qpos)
self.sim.step()
# Change the base link name
self._base_link_name = "right_ee_link"
# Load manger
controllable_wrapper = self.sim.create_controllable_articulation(
self.robot)
self.manger = sapyen_robot.ControllerManger("movo",
controllable_wrapper)
def _load_controller(self) -> None:
"""
Create controllers, set pd and force limit to each joint with fine tuned value
"""
controllable_wrapper = self.sim.create_controllable_articulation(
self.robot)
self._head_joint = ["pan_joint", "tilt_joint"]
self._gripper_joint = [
"right_gripper_finger1_joint", "right_gripper_finger2_joint",
"right_gripper_finger3_joint"
]
self._body_joint = ["linear_joint"]
self.manger = sapyen_robot.ControllerManger("movo",
controllable_wrapper)
self.head_controller = self.manger.create_joint_velocity_controller(
self._head_joint, "head")
self.gripper_controller = self.manger.create_joint_velocity_controller(
self._gripper_joint, "gripper")
self.body_controller = self.manger.create_joint_velocity_controller(
self._body_joint, "body")
# Add joint state publisher to keep in synchronization with ROS
# You must use it if you want to do cartesian control
self.manger.add_joint_state_publisher(60)
self.manger.add_group_trajectory_controller("right_arm")
self.arm_planner = self.manger.create_group_planner("right_arm")
# Cache gripper limit for execute high level action
joint_limit = self.robot.get_qlimits()
gripper_index = self.robot_joint_names.index(self._gripper_joint[0])
self.__gripper_limit = joint_limit[gripper_index, :]
# Cache robot pose
self.root_theta = 0
self.root_pos = np.array([0, 0], dtype=np.float)
self.init_qpos = [
0, 0, 0, 0.25, -1.9347, 0, -1.5318, 0, 0.9512, -2.24, 0.34, 0.64,
-1.413, 0, 0, 0
]
# Tune PD controller
self.robot.set_pd(20000, 3000, 2000, np.arange(4))
self.robot.set_drive_qpos(self.init_qpos)
self.robot.set_qpos(self.init_qpos)
self.sim.step()
def _load_controller_parameters(self):
self._ee_link_name = "panda_link8"
self.root_theta = 0
self.root_pos = np.array([0, 0], dtype=np.float)
self.init_qpos = np.array(
[0.01, -0.664, -0.1, -2.218, 0.203, 1.854, -2.696, 0, 0])
# Tune PD controller
self.robot.set_pd(2000, 300, 300, np.arange(7))
self.robot.set_pd(500000000, 80000, 20000, np.arange(7, 9))
self.robot.set_qpos(self.init_qpos)
self.sim.step()
self.robot.set_drive_qpos(self.init_qpos)
# Create manger
controllable_wrapper = self.sim.create_controllable_articulation(
self.robot) # Cache robot pose
self.manger = sapyen_robot.ControllerManger("arm",
controllable_wrapper)
def _load_controller_parameters(self):
self._ee_link_name = "link6"
self.root_theta = 0
self.root_pos = np.array([0, 0], dtype=np.float)
self.init_qpos = np.array(
[-0.672, -0.454, -0.322, -0.8, -1.045, 0.55, 0, 0, 0, 0, 0, 0])
# Tune PD controller
self.robot.set_pd(2000, 300, 300, np.arange(6))
self.robot.set_pd(500000, 80000, 20000, np.arange(6, 12))
self.robot.set_qpos(self.init_qpos)
self.sim.step()
self.robot.set_drive_qpos(self.init_qpos)
# Create manger
controllable_wrapper = self.sim.create_controllable_articulation(
self.robot) # Cache robot pose
self.manger = sapyen_robot.ControllerManger("xarm6",
controllable_wrapper)
def __init__(self, partnet_id: str):
# Rendering
self.renderer = sapyen.OptifuserRenderer()
self.renderer.set_ambient_light([.4, .4, .4])
self.renderer.set_shadow_light([1, -1, -1], [.5, .5, .5])
self.renderer.add_point_light([2, 2, 2], [1, 1, 1])
self.renderer.add_point_light([2, -2, 2], [1, 1, 1])
self.renderer.add_point_light([-2, 0, 2], [1, 1, 1])
self.renderer.cam.set_position([0.5, -4, 0.5])
self.renderer.cam.set_forward([0, 1, 0])
self.renderer.cam.set_up([0, 0, 1])
# Simulation
self.sim = sapyen.Simulation()
self.sim.set_renderer(self.renderer)
self.sim.set_time_step(1.0 / 200)
self.sim.add_ground(0, material=None)
# Partnet Object
urdf = os.path.join(PARTNET_DIR, partnet_id, "mobility.urdf")
self.loader = self.sim.create_urdf_loader()
self.obj = self.loader.load(urdf)
self.obj.set_root_pose([3, 0, 0.5], [1, 0, 0, 0])
# Robot Model and controller manger
self.loader.fix_loaded_object = True
self.loader.balance_passive_force = True
self.robot = self.loader.load('../assets/robot/all_robot.urdf')
self.robot.set_drive_qpos([0.25, -1.9347, 0, -1.5318, 0, 0.9512, -2.24, 0.34, 0.64, -1.413, 0, 0, 0])
self.robot.set_pd(5000, 1500, 50000)
controllable_wrapper = self.sim.create_controllable_articulation(self.robot)
self.manger = sapyen_robot.ControllerManger("movo", controllable_wrapper)
self.ps3 = sapyen_robot.MOVOPS3(self.manger)
# Init
self.renderer.show_window()
self.ps3.set_demonstration_mode()
self.dump_data = []
self.control_signal = []
self.object_force_array = []
self.robot_force_array = []
def _load_controller_parameters(self):
# Cache robot pose
self.root_theta = 0
self.root_pos = np.array([0, 0], dtype=np.float)
self.robot.set_root_pose([-1, 2, 0.06])
self.init_qpos = [
0.25, -1.381, 0, 0.05, -0.9512, 0.387, 0.608, 2.486, 1.05, -1.16,
0, 0, 0
]
# Tune PD controller
self.robot.set_pd(20000, 3000, 2000, np.arange(1))
self.robot.set_pd(2000, 300, 300, [1, 3, 5, 6, 7, 8, 9])
self.robot.set_pd(500, 100, 300, [2, 4])
self.robot.set_pd(200, 40, 20, np.arange(10, 13))
self.robot.set_qpos(self.init_qpos)
self.sim.step()
# Create manger
controllable_wrapper = self.sim.create_controllable_articulation(
self.robot)
self.manger = sapyen_robot.ControllerManger("movo",
controllable_wrapper)
def __init__(self, partnet_id, mode="state"):
super().__init__(partnet_id)
# Replay mode can be state based or control based
self.mode = mode
if mode == "control":
self.robot.set_drive_qpos([
0.25, -1.9347, 0, -1.5318, 0, 0.9512, -2.24, 0.34, 0.64,
-1.413, 0, 0, 0
])
self.robot.set_pd(5000, 1500, 50000)
controllable_wrapper = self.sim.create_controllable_articulation(
self.robot)
self.manger = sapyen_robot.ControllerManger(
"movo", controllable_wrapper)
self.ps3 = sapyen_robot.MOVOPS3(self.manger)
self.ps3.set_replay_mode()
elif mode == "state":
pass
else:
print("Possible mode is {} and {}".format("control", "state"))
raise Exception("Mode is invalid: {}".format(mode))
# ROS camera specification
self.camera_frame_id = []
self.camera_pose = []
self.pub_list = []
self.__POINT_CLOUD_NAMESPACE = "sapien/replay/"
self.init_camera_ros()
# Add Point Cloud2 template
self.rgb_cloud = None
self.cloud = None
self.add_ros_cloud_template()
# Init ROS node
rospy.init_node("sapien")
def _load_ros_controller(self):
"""
Pure virtual function for loading controllers. It should always be overloaded by child class
"""
self.manger = sapyen_robot.ControllerManger("virtual", None)
raise NotImplementedError