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',
