def set_kv(self, kv=None, scale=1): """ Sets the velocity feedback gain for joint velocity control by writing the specified values to "panda_velocity_control.xml". Args: kv: Velocity feedback gain scale: Scales the Velocity feedback gain Returns: No return value """ if kv is None: kv = [12, 12, 12, 12, 6, 5, 3] else: assert len(kv) == 7, ("Error, the number of entries in <kv> has to match the number of joints (i.e. 7). " "Given: ", len(kv)) tree = Et.parse(sim_framework_path(self._xml_path, 'panda_velocity_control.xml')) root = tree.getroot() velocities = root.find('actuator').findall('velocity') for i, vel in enumerate(velocities): vel.set('kv', str(scale * kv[i])) tree.write(sim_framework_path(self._xml_path, 'panda_velocity_control.xml'))
def set_kp(self, kp=None, scale=1): """ Sets the position feedback gain for joint position control by writing the specified values to "panda_position_control.xml". Args: kp: Position feedback gain scale: Scales the Position feedback gain Returns: No return value """ if kp is None: kp = [870, 870, 870, 870, 120, 120, 120] else: assert len(kp) == 7, ("Error, the number of entries in <kp> has to match the number of joints (i.e. 7). " "Given: ", len(kp)) tree = Et.parse(sim_framework_path(self._xml_path, 'panda_position_control.xml')) root = tree.getroot() positions = root.find('actuator').findall('position') for i, pos in enumerate(positions): pos.set('kp', str(scale * kp[i])) tree.write(sim_framework_path(self._xml_path, 'panda_position_control.xml'))
def __init__(self, panda_xml_path='./envs/mujoco/panda/panda_with_cam_mujoco.xml'): src = sim_framework_path(panda_xml_path) self.scene_xml = sim_framework_path('./envs/mujoco/panda/main_scene.xml') copyfile(src, self.scene_xml) self._xml_path = sim_framework_path('./envs/mujoco/panda/') self._tree = Et.parse(sim_framework_path(self._xml_path, 'main_scene.xml')) self._root = self._tree.getroot() self._worldbody = self._root.find('worldbody') assert self._worldbody, 'Error, xml file does contain a world body.'
def __init__(self, scene, config_path=None, gravity_comp=True, clip_actions=False, num_DoF=7): if config_path is None: config_path = sim_framework_path('./classic_framework/controllers/Config/Mujoco/Standard') super(MujocoRobot, self).__init__(config_path, num_DoF=num_DoF, dt=scene.dt) self.scene = scene self.control_mode = self.scene.control.ctrl_name self.functions = mujoco_py.functions self.sim = scene.sim self.model = scene.model self.viewer = scene.viewer self.render = scene.render self.clip_actions = clip_actions self.gravity_comp = gravity_comp self.gotoJointController = GotoJointController(self.dt) self.gotoCartPosController = GotoCartPosQuatPlanningController(self.dt) self.gotoCartPosController.fictive_robot.offset = np.array([0., 0., 0.]) self.gotoCartPosQuatController = GotoCartPosQuatPlanningController(self.dt) self.gotoCartPosQuatController.fictive_robot.offset = np.array([0., 0., 0.]) self.gotoCartPosImpedanceController = GotoCartPosQuatImpedanceController(self.dt) self.jointTrajectoryTracker = JointTrajectoryTracker(self.dt) # self.mocap_setup = None self.reset()
def __init__(self, urdf_name, object_name, position, orientation, data_dir=None): if data_dir is None: data_dir = sim_framework_path('./envs/data/') objects = os.listdir(data_dir) if not urdf_name.endswith('.urdf'): if urdf_name + '.urdf' in objects: urdf_name += '.urdf' else: raise ValueError("Error, object with name " + urdf_name + " not found. Check that a object with the " "specified name exists in the data " "directory") assert len( position) == 3, "Error, <position> has three entries x, y, z." assert len( orientation ) == 3, "Error, <orientation> has three entries yaw, pitch, and roll." self.__urdf_name = urdf_name self.__object_name = object_name self.__position = position self.__orientation = orientation self.__data_dir = data_dir
def set_damping(self, damping): defaults = self._root.find('default').findall('default') for default in defaults: if default.attrib['class'] == 'panda:body': for atr in default: if atr.tag == 'joint': atr.attrib['damping'] = str(damping) self._tree.write(sim_framework_path(self._xml_path, 'main_scene.xml')) # writes the changes to the file
def load_mj_loadable(self, mj_loadable: MujocoLoadable): xml_element, include = mj_loadable.to_xml(self._xml_path) if include: self._root.append(xml_element) else: self._worldbody.append(xml_element) # append the new object to the worldbody of the xml file self.indent(self._root) # ensures correct indendation self._tree.write(sim_framework_path(self._xml_path, 'main_scene.xml')) # writes the changes to the file
def set_gripper_control(self, control, kv=None, kv_scale=20, kp=None, kp_scale=1): if control.lower() == 'none': return # Set the gripper control gripper_ctrl = Element('actuator') if control == 'ik' or control == 'torque' or control == 'mocap': # inverse kinematics / torque control left_gripper = SubElement(gripper_ctrl, 'motor') # left_gripper.set('ctrlrange', "0 0.04") right_gripper = SubElement(gripper_ctrl, 'motor') # right_gripper.set('ctrlrange', "0 0.04") elif control == 'position': # position control if kp is None: kp = str(kp_scale * 300) left_gripper = SubElement(gripper_ctrl, 'position') left_gripper.set('ctrlrange', "0 0.04") left_gripper.set('kp', kp) right_gripper = SubElement(gripper_ctrl, 'position') right_gripper.set('ctrlrange', "0 0.04") right_gripper.set('kp', kp) elif control == 'velocity': # joint velocity control if kv is None: kv = str(kv_scale) left_gripper = SubElement(gripper_ctrl, 'velocity') left_gripper.set('ctrlrange', "-0.2 0.2") left_gripper.set('ctrllimited', "true") left_gripper.set('kv', kv) right_gripper = SubElement(gripper_ctrl, 'velocity') right_gripper.set('ctrlrange', "-0.2 0.2") right_gripper.set('ctrllimited', "true") right_gripper.set('kv', kv) else: raise ValueError("Error, invalid control value. Choose between <ik> (inverse kinematics), " "<position> (forward kinematics), <torque> (torque based control), <velocity> (joint " "velocity control) or <mocap> (mocap body based control).") left_gripper.set('joint', "panda_finger_joint1") left_gripper.set('name', "panda_finger_joint1_act") right_gripper.set('joint', "panda_finger_joint2") right_gripper.set('name', "panda_finger_joint2_act") right_gripper.set('forcerange', "-70 70") # right_gripper.set('forcelimited', "true") left_gripper.set('forcerange', "-70 70") # left_gripper.set('forcelimited', "true") self._root.append(gripper_ctrl) self.indent(self._root) # ensures correct indentation self._tree.write(sim_framework_path(self._xml_path, 'main_scene.xml')) # writes the changes to the file
def __init__(self, pybullet, scene, config_path=None, gravity_comp=True, pos_ctrl=False, clip_actions=False, num_DoF=7, publisher=None): if config_path is None: config_path = sim_framework_path( './classic_framework/controllers/Config/PyBullet/Standard') super(PyBulletRobot, self).__init__(config_path, num_DoF=num_DoF, dt=scene.dt) self.pybullet = pybullet self.scene = scene self.robot_id, self.robot_id_ik, self.robotEndEffectorIndex = scene.load_panda_to_scene( ) self.pybullet.setTimeStep(self.dt) self.jointIndices_with_fingers = [1, 2, 3, 4, 5, 6, 7, 10, 11] self.pybullet.setJointMotorControlArray( bodyUniqueId=self.robot_id, jointIndices=[10, 11], controlMode=self.pybullet.VELOCITY_CONTROL, forces=[0., 0.]) self.jointIndices = [1, 2, 3, 4, 5, 6, 7] # enable JointForceTorqueSensor for joints # maybe enable joint force sensor for gripper here later!!! [ self.pybullet.enableJointForceTorqueSensor( bodyUniqueId=self.robot_id, jointIndex=jointIndex) for jointIndex in self.jointIndices_with_fingers ] self.gravity_comp = gravity_comp self.pos_ctrl = pos_ctrl if not pos_ctrl: self.scene.disable_robot_vel_ctrl(self.robot_id) else: self.scene.enable_robot_vel_ctrl(self.robot_id, max_forces=self.torque_limit) self.clip_actions = clip_actions self.gotoJointController = GotoJointController(self.dt) self.gotoCartPosController = GotoCartPosQuatPlanningController(self.dt) self.gotoCartPosController.fictive_robot.offset = np.array( [0., 0., 0.88]) self.gotoCartPosQuatController = GotoCartPosQuatPlanningController( self.dt) self.gotoCartPosQuatController.fictive_robot.offset = np.array( [0., 0., 0.88]) self.jointTrajectoryTracker = JointTrajectoryTracker(self.dt) self.receiveState() self.publisher = publisher
def __init__(self, init_j_pos, scene, config_path=None): if config_path is None: config_path = sim_framework_path( './classic_framework/controllers/Config/Mujoco/FictiveRobot') # we have set d-part of orientation to 0 super(MujocoRobot_Fictive, self).__init__(init_j_pos=init_j_pos, config_path=config_path, offset=np.zeros(3), dt=scene.dt) self.scene = scene self.functions = mujoco_py.functions self.receiveState()
def load_npy_file(self, file_name, path_of_file=None): """ load .npy file. path_of_file does not include the file name :param file_name: 'file.npy' :param path_of_file: path to the file, but it does not include the file name itself :return: the numpy obj """ if path_of_file is None: path_of_file = self.config_dir return np.load(sim_framework_path(path_of_file, file_name))
def __init__(self, init_j_pos, dt, offset=0, config_path=None): if config_path is None: config_path = sim_framework_path('./classic_framework/controllers/Config/PyBullet/FictiveRobot') super().__init__(config_path, dt=dt) obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf") self.model = pinocchio.buildModelFromUrdf(obj_urdf) self.data = self.model.createData() # The offset is [0, 0, 0.88] for pybullet (with the table) # The offset is [0, 0, 0] for mujoco (with current scene) # NOTE: adjust the offset (z-direction), if you re-place the robot! (Use check offset function of fictive robot) self.offset = offset self.init_j_pos = init_j_pos self.end_effector_frame_id = self.model.getFrameId('panda_grasptarget') self.des_joint_traj = [] self.gotoJointController = GotoJointController(self.dt) self.gotoCartPosController = GotoCartPosImpedanceController(self.dt) self.gotoCartPosQuatController = GotoCartPosQuatImpedanceController(self.dt) self.jointTrajectoryTracker = JointTrajectoryTracker(self.dt)
def save2npy_file(self, file, name_of_file, path2be_saved=None): """ save file to a .npy file :param file: the obj to be saved :param name_of_file: the name of the .npy file :param path2be_saved: save file to certain directory, if none then save to current project's directory :return: """ if path2be_saved is None: path2be_saved = self.config_dir np.save(sim_framework_path(path2be_saved, name_of_file), file)
def add_mocap(self): """ Creates a mocap body and adds it to the .xml creating the scene. Returns: No return value """ # Create mocap body object_body = Element('body') object_body.set('mocap', 'true') object_body.set('name', 'panda:mocap') object_body.set('pos', '0 0 0') # Set the params of the mocap geom = SubElement(object_body, 'geom') geom.set('conaffinity', "0") geom.set('contype', "0") geom.set('pos', "0 0 0") geom.set('rgba', "0 0.5 0 0.7") geom.set('size', "0.005 0.005 0.005") geom.set('type', "box") geom1 = SubElement(object_body, 'geom') geom1.set('conaffinity', "0") geom1.set('contype', "0") geom1.set('pos', "0 0 0") geom1.set('rgba', "1 0 0 0.3") geom1.set('size', "1 0.005 0.005") geom1.set('type', "box") geom2 = SubElement(object_body, 'geom') geom2.set('conaffinity', "0") geom2.set('contype', "0") geom2.set('pos', "0 0 0") geom2.set('rgba', "0 1 0 0.3") geom2.set('size', "0.005 1 0.005") geom2.set('type', "box") geom3 = SubElement(object_body, 'geom') geom3.set('conaffinity', "0") geom3.set('contype', "0") geom3.set('pos', "0 0 0") geom3.set('rgba', "0 0 1 0.3") geom3.set('size', "0.005 0.005 1") geom3.set('type', "box") # Write the defined mocap to the worldbody of the scene self._worldbody.append(object_body) # append the new object to the worldbody of the xml file self.indent(self._root) # ensures correct indentation self._tree.write(sim_framework_path(self._xml_path, 'main_scene.xml')) # writes the changes to the file
def save2yaml(self, data, name_of_file, path2be_saved=None): """ Method saves data as yaml file to path2be_saved :param data: dictionary to be saved :param name_of_file: name of the file without .yaml :param path2be_saved: path to save the file :return: """ if path2be_saved is None: path2be_saved = self.config_dir with io.open(sim_framework_path(path2be_saved, name_of_file + '.yaml'), 'w') as stream: yaml.dump(data, stream)
def load_yaml(self, file_name, path_of_file=None): """ load .yaml file. path_of_file does not include the file name :param file_name: file name without .yaml ending :param path_of_file: path to the file, but it does not include the file name itself :return: the dictionary """ if path_of_file is None: path_of_file = self.config_dir print(path_of_file) print(file_name + '.yaml') with open(sim_framework_path(path_of_file, file_name + '.yaml'), 'r') as stream: data = yaml.safe_load(stream) return data
def set_dt(self, dt=0.001): """ Sets 1 / number of timesteps mujoco needs for executing one second of wall-clock time by writing to the root xml.file. Args: dt: 1 / number of timesteps needed for computing one second of wall-clock time Returns: No return value """ options = self._root.find('option') options.set('timestep', str(dt)) self.indent(self._root) # ensures correct indentation self._tree.write(sim_framework_path(self._xml_path, 'main_scene.xml')) # writes the changes to the file
def check_offset_to_pinocchio_model_in_cart_space(self, q): import pinocchio from classic_framework.utils.geometric_transformation import mat2quat obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf") model = pinocchio.buildModelFromUrdf(obj_urdf) data = model.createData() q_pin = np.zeros(9) q_pin[:7] = q pinocchio.framesForwardKinematics(model, data, q_pin) pinocchio.computeJointJacobians(model, data, q_pin) pinocchio.framesForwardKinematics(model, data, q_pin) # get orientation rotation_matrix = data.oMf[model.getFrameId('panda_grasptarget')].rotation quaternions = mat2quat(rotation_matrix) # [w x y z] jac = pinocchio.getFrameJacobian(model, data, model.getFrameId('panda_grasptarget'), pinocchio.LOCAL_WORLD_ALIGNED)[:, :7] print('position offset:', self.current_c_pos - np.array(data.oMf[model.getFrameId('panda_grasptarget')].translation))
def setup_scene(self): """ Creates a scene which is read from a .xml file. Returns: No return value """ xml_path = sim_framework_path('./envs/mujoco/panda/main_scene.xml') # xml_path = sim_framework_path('./envs/mujoco/panda/panda_with_cam_original_inertia.xml') # ik control with orientation # has problems when using PD control, # when using inv dynamics control, works very good # adding all assets to the scene for obj in self.object_list: if isinstance(obj, MujocoLoadable): self.xml_parser.load_mj_loadable(obj) for cam in self.camera_list: self.xml_parser.load_mj_loadable(cam) model = load_model_from_path(xml_path) self.sim = MjSim(model=model, nsubsteps=self.n_substeps) self.geom_to_idx = mj_help.get_geom_to_idx_dict(model=model) self.body_to_idx = mj_help.get_body_to_idx_dict(model=model) self.joint_to_idx = mj_help.get_joint_to_idx_dict(model=model) self.site_to_idx = mj_help.get_site_to_idx_dict(model=model) self.load_panda_to_scene() os.remove( xml_path) # remove the created xml file after creating the scene # Show simulation self.viewer = None if self.render: self.viewer = MjViewer(self.sim) self.viewer.render() return self.sim, model, self.viewer
def __init__(self, init_j_pos, pybullet, robotEndEffectorIndex, scene, ik_robot_id, config_path=None): if config_path is None: move_dirs_up = 2 config_path = sim_framework_path( './classic_framework/controllers/Config/PyBullet/FictiveRobot') super().__init__(init_j_pos, config_path=config_path, offset=np.array([0., 0., 0.88]), dt=scene.dt) self.scene = scene self.pybullet = pybullet self.robotEndEffectorIndex = robotEndEffectorIndex self.ik_robot_id = ik_robot_id self.ik_client_id = self.scene.ik_client_id self.receiveState()
def load_panda_to_scene(self, physics_robot=True, orientation=None, position=None, id_name=None, path_to_urdf=None, init_q=None): """ This function loads another panda robot to the simulation environment. If loading the object fails, the program is stopped and an appropriate get_error message is returned to user. :param physics_robot: number of active robots in physics client :param path_to_urdf: the whole path of the location of the urdf file to be load as string. If none use default :param orientation: orientation of the robot. Can be either euler angles, or quaternions. NOTE: quaternions notation: [x, y, z, w] ! :param position: cartesian world position to place the robot :param id_name: string valued name of the robot. This name can then be called as self.'name'_id to get the id number of the object :return: returns the id of the new panda robot """ if position is None: position = [0.0, 0.0, 0.88] if orientation is None: orientation = [0.0, 0.0, 0.0] if path_to_urdf is None: obj_urdf = sim_framework_path( "./envs/frankaemika/robots/panda_arm_hand.urdf" ) # panda robot with inhand camera # obj_urdf = sim_framework_path("./envs/frankaemika/robots/panda_arm_hand_pybullet.urdf") # panda robot with inhand camera # obj_urdf = sim_framework_path("./envs/frankaemika/robots/panda_arm_hand_without_cam.urdf") # obj_urdf = sim_framework_path("./envs/frankaemika/robots/panda_arm_hand_without_cam_inertia_from_mujoco.urdf") else: obj_urdf = path_to_urdf orientation = list(orientation) if len(orientation) == 3: orientation = p.getQuaternionFromEuler(orientation) position = list(position) try: id = p.loadURDF( obj_urdf, position, orientation, useFixedBase=1, # | p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_INERTIA_FROM_FILE, # flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT, physicsClientId=self.physics_client_id) ik_id = p.loadURDF( obj_urdf, position, orientation, useFixedBase=1, flags=p. URDF_USE_SELF_COLLISION, # | p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT physicsClientId=self.ik_client_id) if id_name is not None: setattr(self, id_name + '_id', id) self.obj_name2id[id_name + '_id'] = id else: self.robot_physics_client_id = id self.robot_ik_client_id = ik_id except Exception: print() print('Stopping the program') raise ValueError( 'Could not load URDF-file: Check the path to file. Stopping the program. Your path:', obj_urdf) if init_q is None: init_q = (3.57795216e-09, 1.74532920e-01, 3.30500960e-08, -8.72664630e-01, -1.14096181e-07, 1.22173047e+00, 7.85398126e-01) self.set_q(init_q, id) # robotEndEffectorIndex = 8 # robotEndEffectorIndex = 9 robotEndEffectorIndex = 12 return id, ik_id, robotEndEffectorIndex
def xml_file_path(self): return sim_framework_path( './envs/mujoco/panda/controller/panda_{}_control.xml'.format( self._xml_name))
def _set_control_by_name(self, control_name: str, set_gripper=True, gripper_ctrl=None, kv=None, kv_scale=1, kp=None, kp_scale=1): # TODO: Remove in future! """ Sets the control for the robot by writing the respective file to the xml. Args: control_name: Choose between: <ik> (inverse kinematics), <position> (forward kinematics), <torque> (torque based control) or <velocity> (joint velocity control). kv: Velocity feedback gain for each joint (list with num joints entries) kv_scale: Scales each Velocity feedback gain by a scalar (single value) kp: Position feedback gain for each joint (list with num joints entries) kp_scale: Scales each Position feedback gain by a scalar (single value) Returns: No return value. """ include = Element('include') if control_name == 'ik' or control_name == 'torque': # inverse kinematics / torque control include.set('file', './controller/panda_torque_control.xml') self._root.append(include) if gripper_ctrl is None: gripper_ctrl = 'ik' elif control_name == 'position': # position control self.set_kp(kp=kp, scale=kp_scale) include.set('file', './controller/panda_position_control.xml') self._root.append(include) if gripper_ctrl is None: gripper_ctrl = 'ik' elif control_name == 'velocity': # joint velocity control self.set_kv(kv=kv, scale=kv_scale) include.set('file', './controller/panda_velocity_control.xml') self._root.append(include) if gripper_ctrl is None: gripper_ctrl = 'ik' elif control_name == 'mocap': # mocap control include.set('file', './controller/panda_mocap_control.xml') self._root.append(include) # self.add_mocap() if gripper_ctrl is None: gripper_ctrl = 'ik' else: raise ValueError("Error, invalid control value. Choose between <ik> (inverse kinematics), " "<position> (forward kinematics), <torque> (torque based control), <velocity> (joint " "velocity control) or <mocap> (mocap body based control).") if set_gripper: self.set_gripper_control(control=gripper_ctrl) self.indent(self._root) # ensures correct indentation self._tree.write(sim_framework_path(self._xml_path, 'main_scene.xml')) # writes the changes to the file
def setup_scene(self): """ This function creates a scene. :return: no return value """ print("Scene setup") # Connect with simulator if self.render: self.physics_client = BulletClient( p.GUI) # o r p.DIRECT for non-graphical version else: self.physics_client = BulletClient( p.DIRECT) # o r p.DIRECT for non-graphical version self.physics_client_id = self.physics_client._client self.ik_client = BulletClient(connection_mode=p.DIRECT) self.ik_client_id = self.ik_client._client p.setPhysicsEngineParameter(enableFileCaching=0) # -------------------------------------------------------------------------------------------------------------- # # Load scene # -------------------------------------------------------------------------------------------------------------- # module_path = path.dirname(path.abspath(__file__)) # os.path.abspath(os.curdir) # os.chdir("..") # module_path = os.path.abspath(os.curdir) # os.chdir(os.getcwd() + os.sep + os.pardir) # moves to parent directory # module_path = os.getcwd() if self.object_list is not None: for obj in self.object_list: self.load_object_to_scene(path_to_urdf=obj.data_dir + "/" + obj.urdf_name, orientation=obj.orientation, position=obj.position, id_name=obj.object_name) self.scene_id = p.loadURDF( sim_framework_path("./envs/plane/plane.urdf"), physicsClientId=self.physics_client_id) self.scene_id_ik = p.loadURDF( sim_framework_path("./envs/plane/plane.urdf"), physicsClientId=self.ik_client_id) # load table table_urdf = sim_framework_path("./envs/table/table.urdf") table_start_position = [0.35, 0.0, 0.0] table_start_orientation = [0.0, 0.0, 0.0] table_start_orientation_quat = p.getQuaternionFromEuler( table_start_orientation) self.table_id = p.loadURDF(table_urdf, table_start_position, table_start_orientation_quat, flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_INERTIA_FROM_FILE, physicsClientId=self.physics_client_id) self.table_id_ik = p.loadURDF(table_urdf, table_start_position, table_start_orientation_quat, flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_INERTIA_FROM_FILE, physicsClientId=self.ik_client_id) p.setGravity(0, 0, -9.81)
def xml_file_path(self): return sim_framework_path('gym_framework', 'mujoco_objects', 'assets', 'box.xml')
from classic_framework.mujoco.MujocoRobot import MujocoRobot from classic_framework.mujoco.MujocoScene import MujocoScene as Scene from classic_framework.interface.Logger import RobotPlotFlags from classic_framework.utils.sim_path import sim_framework_path import numpy as np if __name__ == '__main__': object_list = [] duration = 4 # Setup the scene scene = Scene(object_list=object_list) mj_Robot = MujocoRobot(scene, gravity_comp=True, num_DoF=7) mj_Robot.startLogging() # load the trajectory you want to follow path2file = sim_framework_path( 'demos/mujoco/robot_control_scene_creation/des_joint_traj.npy') des_joint_trajectory = np.load(path2file) mj_Robot.follow_JointTraj(des_joint_trajectory) mj_Robot.stopLogging() mj_Robot.logger.plot(plotSelection=RobotPlotFlags.JOINTS)
2.3.2 executeController 2.3.2.1 robot.nextStep 2.3.2.2 robot.receiveState 2.3.2.3 while not finished: getControl and nextStep """ import numpy as np import pybullet as p from classic_framework.pybullet.pb_utils.pybullet_scene_object import PyBulletObject from classic_framework import RobotPlotFlags from classic_framework.pybullet.PyBulletRobot import PyBulletRobot from classic_framework.pybullet.PyBulletScene import PyBulletScene as Scene from classic_framework.utils.sim_path import sim_framework_path if __name__ == '__main__': data_dir = sim_framework_path('./envs/data') cube1 = PyBulletObject(urdf_name='cuboid', object_name='cube_1', position=[6.5e-01, 0.01, 0.91], orientation=[np.pi / 2, 0, 0], data_dir=None) cube2 = PyBulletObject(urdf_name='cuboid', object_name='cube_2', position=[6.5e-01, -0.2, 0.91], orientation=[np.pi / 2, 0, 0], data_dir=None) duck = PyBulletObject(urdf_name='duck_vhacd', object_name='duck1',
def __init__(self, object_list=None, camera_list=None, panda_xml_path=None, control: mj_ctrl.MujocoController = None, gripper_control=None, dt=0.001, n_substeps=1, kp=None, kp_scale=1, kv=None, kv_scale=1, render=True): super(MujocoScene, self).__init__(object_list=object_list, dt=dt, render=render) """ Initialises the scene. Args: object_list: List with :class:`MujocoObject`s camera_list: List with :class:`Camera`s control: A MujocoController implementation dt: 1 / number of timesteps needed for computing one second of wall-clock time kv: Velocity feedback gain for each joint (list with num joints entries) kv_scale: Scales each Velocity feedback gain by a scalar (single value) kp: Position feedback gain for each joint (list with num joints entries) kp_scale: Scales each Position feedback gain by a scalar (single value) """ if control is None: control = mj_ctrl.IKControl() elif isinstance(control, str): control = mj_ctrl.convert_str2control(control) if panda_xml_path is None: panda_xml_path = sim_framework_path( './envs/mujoco/panda/panda_with_cam_mujoco.xml') if control.ctrl_name == 'mocap': panda_xml_path = sim_framework_path( './envs/mujoco/panda/panda_mocap_model.xml') if camera_list is None: camera_list = [] if object_list is None: object_list = [] if not isinstance(object_list, list): object_list = [object_list] if not isinstance(camera_list, list): camera_list = [camera_list] self.control = control self.panda_xml_path = panda_xml_path self.xml_parser = MujocoSceneParser(panda_xml_path=panda_xml_path) self.n_substeps = n_substeps self.xml_parser.set_dt(self.dt) if control is not None: self.xml_parser.set_control(control=control, gripper_ctrl=gripper_control, kp=kp, kp_scale=kp_scale, kv=kv, kv_scale=kv_scale) self.init_qpos = None self.init_qvel = None self.body_to_idx = None self.joint_to_idx = None self.site_to_idx = None self.geom_to_idx = None self.camera_list = camera_list self.object_list = object_list self.sim, self.model, self.viewer = self.setup_scene() self.inhand_cam = Camera(self.sim, name='rgbd', fovy=45, ipd=0.068) self.cage_cam = Camera(self.sim, name='rgbd_cage', fovy=45, ipd=0.068) self.cameras = {'rgbd': self.inhand_cam, 'rgbd_cage': self.cage_cam} self.joint_names = [ name for name in self.sim.model.joint_names if name.startswith('panda_joint') ] assert len( self.joint_names) == 7, "Error, found more joints than expected." self.gripper_names = [ name for name in self.sim.model.joint_names if name.startswith('panda_finger_joint') ] assert len(self.gripper_names ) == 2, "Error, found more gripper joints than expected." for cam in camera_list: self.cameras[cam.cam_name] = Camera(sim=self.sim, name=cam.cam_name, fovy=float(cam.fovy), ipd=cam.ipd)
2.3.2 executeController 2.3.2.1 robot.nextStep 2.3.2.2 robot.receiveState 2.3.2.3 while not finished: getControl and nextStep """ import numpy as np import pybullet as p from classic_framework import RobotPlotFlags from classic_framework.pybullet.PyBulletRobot import PyBulletRobot from classic_framework.pybullet.PyBulletScene import PyBulletScene as Scene from classic_framework.pybullet.pb_utils.pybullet_scene_object import PyBulletObject from classic_framework.utils.sim_path import sim_framework_path if __name__ == '__main__': data_dir = sim_framework_path('envs', 'data') cube1 = PyBulletObject(urdf_name='cuboid', object_name='cube_1', position=[6.5e-01, 0.01, 0.91], orientation=[np.pi / 2, 0, 0], data_dir=None) cube2 = PyBulletObject(urdf_name='cuboid', object_name='cube_2', position=[6.5e-01, -0.2, 0.91], orientation=[np.pi / 2, 0, 0], data_dir=None) duck = PyBulletObject(urdf_name='duck_vhacd', object_name='duck1',