mirror of
https://github.com/wassname/pyrobolearn.git
synced 2026-09-11 12:31:07 +08:00
update mujoco interface
This commit is contained in:
Brian Delhaisse
+146
-128
@@ -191,7 +191,7 @@ class Body(object):
|
||||
# keep in memory the link ids
|
||||
|
||||
# define variables related to actuators and control
|
||||
self.ctrl_mode = struct.ControlMode.NULL # remember the last control mode
|
||||
self.ctrl_modes = np.array([struct.ControlMode.NULL] * self.num_actuated_joints) # one for each joint
|
||||
self.gains = None # original gains
|
||||
self.biases = None # original biases
|
||||
self.ctrl_limited = None # original binary vector (nu,) to specify if the control inputs are limited
|
||||
@@ -416,7 +416,14 @@ class Body(object):
|
||||
return self.tag.attrib.get("name")
|
||||
|
||||
def get_q_idx(self, joint_id, keep=False):
|
||||
"""Return the q index(ices) associated with the given joint id(s).
|
||||
"""
|
||||
Return the q index(ices) associated with the given joint id(s). The indices are between -1 and the number of
|
||||
actuated joints. If the provided `keep` argument is True, then it will return also the q indices for the
|
||||
fixed joints. By default, for the fixed joints, the corresponding q index is set to -1.
|
||||
|
||||
For instance, if we have a body with 5 joints such that in the order we have 1 revolute, 2 fixed, and 2
|
||||
revolute joints, the method `get_q_idx` (with `keep=True` and specifying all the joint ids even the fixed
|
||||
ones) will return [0, -1, -1, 1, 2]. If `keep=False`, then it will return [0, 1, 2].
|
||||
|
||||
Args:
|
||||
joint_id (np.array[int], int): joint id(s) (each joint id should be between [0, num_joints[).
|
||||
@@ -1376,12 +1383,22 @@ class Mujoco(Simulator):
|
||||
print(self._parser.get_string(pretty_format=True))
|
||||
|
||||
# save default gain and bias parameters, control inputs range, and force range
|
||||
# body.gains = self.model.actuator_gainprm[body.u_idx0:body.u_idxf]
|
||||
# body.biases = self.model.actuator_biasprm[body.u_idx0:body.u_idxf]
|
||||
# body.ctrl_limited = self.model.actuator_ctrllimited[body.u_idx0:body.u_idxf]
|
||||
# body.force_limited = self.model.actuator_forcelimited[body.u_idx0:body.u_idxf]
|
||||
# body.ctrl_range = self.model.actuator_ctrlrange[body.u_idx0:body.u_idxf]
|
||||
# body.force_range = self.model.actuator_forcerange[body.u_idx0:body.u_idxf]
|
||||
body.gains = self.model.actuator_gainprm[body.u_idx0:body.u_idxf]
|
||||
body.biases = self.model.actuator_biasprm[body.u_idx0:body.u_idxf]
|
||||
body.ctrl_limited = self.model.actuator_ctrllimited[body.u_idx0:body.u_idxf]
|
||||
body.force_limited = self.model.actuator_forcelimited[body.u_idx0:body.u_idxf]
|
||||
body.ctrl_range = self.model.actuator_ctrlrange[body.u_idx0:body.u_idxf]
|
||||
body.force_range = self.model.actuator_forcerange[body.u_idx0:body.u_idxf]
|
||||
|
||||
# print("Creating body with the following gains: ")
|
||||
# print("Position gain = {}, bias = {}".format(self.model.actuator_gainprm[body.u_p_indices],
|
||||
# self.model.actuator_biasprm[body.u_p_indices]))
|
||||
# print("Velocity gain = {}, bias = {}".format(self.model.actuator_gainprm[body.u_v_indices],
|
||||
# self.model.actuator_biasprm[body.u_v_indices]))
|
||||
# print("Effort gain = {}, bias = {}".format(self.model.actuator_gainprm[body.u_e_indices],
|
||||
# self.model.actuator_biasprm[body.u_e_indices]))
|
||||
# print("Force limited?: ", self.model.actuator_forcelimited[body.u_p_indices])
|
||||
# print("Force range = ", self.model.actuator_forcerange[body.u_p_indices])
|
||||
|
||||
# return body id
|
||||
return body_id
|
||||
@@ -2841,66 +2858,74 @@ class Mujoco(Simulator):
|
||||
# # self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = tau + c_q_dq # DEPRECATED
|
||||
# self.sim.data.qfrc_actuator[body.v_idx1 + q[q != -1]] = tau + c_q_dq
|
||||
|
||||
# TODO: use the given kps and kds
|
||||
# get body and q indices
|
||||
body = self._bodies[body_id]
|
||||
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if velocities is None and body.ctrl_mode != struct.ControlMode.POSITION:
|
||||
# Switch to position control mode
|
||||
body_p_indices = body.u_p_indices - body.u_idx0
|
||||
self.model.actuator_gainprm[body.u_p_indices, 0] = body.gains[body_p_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_p_indices, 1] = -body.biases[body_p_indices, 1]
|
||||
self.model.actuator_gainprm[body.u_v_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_v_indices] = 0
|
||||
self.model.actuator_gainprm[body.u_e_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices] = 0
|
||||
body.ctrl_mode = struct.ControlMode.POSITION
|
||||
elif velocities is not None and body.ctrl_mode != struct.ControlMode.PD:
|
||||
# Switch to PD control mode
|
||||
body_p_indices = body.u_p_indices - body.u_idx0
|
||||
body_v_indices = body_p_indices + 1
|
||||
self.model.actuator_gainprm[body.u_p_indices, 0] = body.gains[body_p_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_p_indices, 1] = -body.biases[body_p_indices, 1]
|
||||
self.model.actuator_gainprm[body.u_v_indices, 0] = body.gains[body_v_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_v_indices, 2] = -body.gains[body_v_indices, 2]
|
||||
self.model.actuator_gainprm[body.u_e_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices] = 0
|
||||
body.ctrl_mode = struct.ControlMode.PD
|
||||
|
||||
if forces is not None:
|
||||
self.model.actuator_forcelimited[body.u_p_indices] = 1
|
||||
self.model.actuator_forcerange[body.u_p_indices, 0] = -forces
|
||||
self.model.actuator_forcerange[body.u_p_indices, 1] = forces
|
||||
# TODO: reset force range when None
|
||||
|
||||
if velocities is not None:
|
||||
self.model.actuator_ctrllimited[body.u_v_indices] = 1
|
||||
self.model.actuator_ctrlrange[body.u_v_indices, 0] = -velocities
|
||||
self.model.actuator_ctrlrange[body.u_v_indices, 1] = velocities
|
||||
# TODO: reset velocity range when None
|
||||
|
||||
if joint_ids is None:
|
||||
self.sim.data.ctrl[body.u_p_indices] = positions
|
||||
if velocities is not None:
|
||||
self.sim.data.ctrl[body.u_v_indices] = velocities
|
||||
|
||||
q_idx = np.array(range(body.num_actuated_joints))
|
||||
else:
|
||||
# check if valid joints
|
||||
self._check_joint_ids(body, joint_ids)
|
||||
|
||||
# if one joint, set its position
|
||||
if isinstance(joint_ids, int):
|
||||
self.sim.data.ctrl[body.u_p_indices[joint_ids]] = positions
|
||||
if velocities is not None:
|
||||
self.sim.data.ctrl[body.u_v_indices[joint_ids]] = velocities
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = q_idx[q_idx != -1]
|
||||
|
||||
# if multiple joints, set their positions
|
||||
else:
|
||||
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q = q[q != -1]
|
||||
self.sim.data.ctrl[body.u_p_indices + q] = positions
|
||||
if velocities is not None:
|
||||
self.sim.data.ctrl[body.u_v_indices + q] = velocities
|
||||
if len(q_idx) == 0:
|
||||
raise ValueError("No actuated joints to set the positions to...")
|
||||
|
||||
# set gains
|
||||
if kps is not None:
|
||||
self.model.actuator_gainprm[body.u_p_indices[q_idx], 0] = kps # for desired positions
|
||||
self.model.actuator_biasprm[body.u_p_indices[q_idx], 1] = -kps # for current positions
|
||||
if kds is not None:
|
||||
if velocities is not None:
|
||||
self.model.actuator_gainprm[body.u_v_indices[q_idx], 0] = kds # for desired velocities
|
||||
self.model.actuator_biasprm[body.u_v_indices[q_idx], 2] = -kds # for current velocities
|
||||
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if velocities is None and np.any(body.ctrl_modes[q_idx] != struct.ControlMode.POSITION):
|
||||
# Switch to position control mode
|
||||
body.ctrl_modes[q_idx] = struct.ControlMode.POSITION
|
||||
|
||||
body_p_indices = body.u_p_indices - body.u_idx0
|
||||
if kps is None: # if gains were not provided, set back to the default ones
|
||||
self.model.actuator_gainprm[body.u_p_indices[q_idx], 0] = body.gains[body_p_indices[q_idx], 0]
|
||||
self.model.actuator_biasprm[body.u_p_indices[q_idx], 1] = body.biases[body_p_indices[q_idx], 1]
|
||||
self.model.actuator_gainprm[body.u_v_indices[q_idx]] = 0
|
||||
self.model.actuator_biasprm[body.u_v_indices[q_idx]] = 0
|
||||
self.model.actuator_gainprm[body.u_e_indices[q_idx]] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices[q_idx]] = 0
|
||||
elif velocities is not None and np.any(body.ctrl_mode[q_idx] != struct.ControlMode.PD):
|
||||
# Switch to PD control mode
|
||||
body.ctrl_modes[q_idx] = struct.ControlMode.PD
|
||||
|
||||
body_p_indices = body.u_p_indices - body.u_idx0
|
||||
body_v_indices = body_p_indices + 1
|
||||
if kps is None: # if gains were not provided, set back to the default ones
|
||||
self.model.actuator_gainprm[body.u_p_indices[q_idx], 0] = body.gains[body_p_indices[q_idx], 0]
|
||||
self.model.actuator_biasprm[body.u_p_indices[q_idx], 1] = body.biases[body_p_indices[q_idx], 1]
|
||||
if kds is None: # if gains were not provided, set back to the default ones
|
||||
self.model.actuator_gainprm[body.u_v_indices[q_idx], 0] = body.gains[body_v_indices[q_idx], 0]
|
||||
self.model.actuator_biasprm[body.u_v_indices[q_idx], 2] = body.gains[body_v_indices[q_idx], 2]
|
||||
self.model.actuator_gainprm[body.u_e_indices[q_idx]] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices[q_idx]] = 0
|
||||
|
||||
# set max forces
|
||||
if forces is None: # TODO: improve this part
|
||||
body_p_indices = body.u_p_indices - body.u_idx0
|
||||
self.model.actuator_forcelimited[body.u_p_indices[q_idx]] = body.force_limited[body_p_indices[q_idx]]
|
||||
self.model.actuator_forcerange[body.u_p_indices[q_idx], 0] = -body.force_range[body_p_indices[q_idx], 0]
|
||||
self.model.actuator_forcerange[body.u_p_indices[q_idx], 1] = body.force_range[body_p_indices[q_idx], 1]
|
||||
else:
|
||||
self.model.actuator_forcelimited[body.u_p_indices[q_idx]] = 1
|
||||
self.model.actuator_forcerange[body.u_p_indices[q_idx], 0] = -forces
|
||||
self.model.actuator_forcerange[body.u_p_indices[q_idx], 1] = forces
|
||||
|
||||
# set joint positions
|
||||
self.sim.data.ctrl[body.u_p_indices[q_idx]] = positions
|
||||
|
||||
# set joint velocities if specified as well
|
||||
if velocities is not None:
|
||||
self.sim.data.ctrl[body.u_v_indices[q_idx]] = velocities
|
||||
|
||||
def get_joint_positions(self, body_id, joint_ids=None):
|
||||
"""
|
||||
@@ -2930,9 +2955,9 @@ class Mujoco(Simulator):
|
||||
return self.sim.data.qpos[body.q_idx1 + joint_ids]
|
||||
|
||||
# if multiple joints, return their positions
|
||||
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
qpos = np.zeros(len(joint_ids))
|
||||
qpos[q != -1] = self.sim.data.qpos[body.q_idx1 + q[q != -1]]
|
||||
qpos[q_idx != -1] = self.sim.data.qpos[body.q_idx1 + q_idx[q_idx != -1]]
|
||||
return qpos
|
||||
|
||||
def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
|
||||
@@ -2962,41 +2987,40 @@ class Mujoco(Simulator):
|
||||
# q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
# self.sim.data.qvel[body.v_idx1 + q[q != -1]] = velocities
|
||||
|
||||
# get body and q indices
|
||||
body = self._bodies[body_id]
|
||||
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if body.ctrl_mode != struct.ControlMode.VELOCITY:
|
||||
# Switch to velocity control mode
|
||||
body.ctrl_mode = struct.ControlMode.VELOCITY
|
||||
body_v_indices = body.u_v_indices - body.u_idx0
|
||||
self.model.actuator_gainprm[body.u_p_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_p_indices] = 0
|
||||
self.model.actuator_gainprm[body.u_v_indices, 0] = body.gains[body_v_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_v_indices, 2] = body.biases[body_v_indices, 2]
|
||||
self.model.actuator_gainprm[body.u_e_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices] = 0
|
||||
|
||||
if max_force is not None:
|
||||
self.model.actuator_forcelimited[body.u_v_indices] = 1
|
||||
self.model.actuator_forcerange[body.u_v_indices, 0] = -max_force
|
||||
self.model.actuator_forcerange[body.u_v_indices, 1] = max_force
|
||||
# TODO: reset force range when None
|
||||
|
||||
if joint_ids is None:
|
||||
self.sim.data.ctrl[body.u_v_indices] = velocities
|
||||
|
||||
q_idx = np.array(range(body.num_actuated_joints))
|
||||
else:
|
||||
# check if valid joints
|
||||
self._check_joint_ids(body, joint_ids)
|
||||
|
||||
# if one joint, set its velocities
|
||||
if isinstance(joint_ids, int):
|
||||
self.sim.data.ctrl[body.u_v_indices[joint_ids]] = velocities
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = q_idx[q_idx != -1]
|
||||
|
||||
# if multiple joints, set their velocities
|
||||
else:
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
self.sim.data.ctrl[body.u_v_indices + q_idx[q_idx != -1]] = velocities
|
||||
if len(q_idx) == 0:
|
||||
raise ValueError("No actuated joints to set the velocities to...")
|
||||
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if np.any(body.ctrl_modes[q_idx] != struct.ControlMode.VELOCITY):
|
||||
# Switch to velocity control mode
|
||||
body.ctrl_modes[q_idx] = struct.ControlMode.VELOCITY
|
||||
body_v_indices = body.u_v_indices - body.u_idx0
|
||||
self.model.actuator_gainprm[body.u_p_indices[q_idx]] = 0
|
||||
self.model.actuator_biasprm[body.u_p_indices[q_idx]] = 0
|
||||
self.model.actuator_gainprm[body.u_v_indices[q_idx], 0] = body.gains[body_v_indices[q_idx], 0]
|
||||
self.model.actuator_biasprm[body.u_v_indices[q_idx], 2] = body.biases[body_v_indices[q_idx], 2]
|
||||
self.model.actuator_gainprm[body.u_e_indices[q_idx]] = 0
|
||||
self.model.actuator_biasprm[body.u_e_indices[q_idx]] = 0
|
||||
|
||||
if max_force is not None:
|
||||
self.model.actuator_forcelimited[body.u_v_indices[q_idx]] = 1
|
||||
self.model.actuator_forcerange[body.u_v_indices[q_idx], 0] = -max_force
|
||||
self.model.actuator_forcerange[body.u_v_indices[q_idx], 1] = max_force
|
||||
# TODO: reset force range when None
|
||||
|
||||
# set joint velocities
|
||||
self.sim.data.ctrl[body.u_v_indices[q_idx]] = velocities
|
||||
|
||||
def get_joint_velocities(self, body_id, joint_ids=None):
|
||||
"""
|
||||
@@ -3025,9 +3049,9 @@ class Mujoco(Simulator):
|
||||
return self.sim.data.qvel[body.v_idx1 + joint_ids]
|
||||
|
||||
# if multiple joints, return their velocities
|
||||
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
qvel = np.zeros(len(joint_ids))
|
||||
qvel[q != -1] = self.sim.data.qvel[body.v_idx1 + q[q != -1]]
|
||||
qvel[q_idx != -1] = self.sim.data.qvel[body.v_idx1 + q_idx[q_idx != -1]]
|
||||
return qvel
|
||||
|
||||
def set_joint_accelerations(self, body_id, joint_ids, accelerations, q=None, dq=None):
|
||||
@@ -3102,46 +3126,40 @@ class Mujoco(Simulator):
|
||||
# self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = torques
|
||||
|
||||
body = self._bodies[body_id]
|
||||
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if body.ctrl_mode != struct.ControlMode.EFFORT:
|
||||
# Switch to effort control mode
|
||||
body.ctrl_mode = struct.ControlMode.EFFORT
|
||||
body_e_indices = body.u_e_indices - body.u_idx0
|
||||
self.model.actuator_gainprm[body.u_p_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_p_indices] = 0
|
||||
self.model.actuator_gainprm[body.u_v_indices] = 0
|
||||
self.model.actuator_biasprm[body.u_v_indices] = 0
|
||||
self.model.actuator_gainprm[body.u_e_indices, 0] = body.gains[body_e_indices, 0]
|
||||
self.model.actuator_biasprm[body.u_e_indices] = 0
|
||||
|
||||
# reset to original control and force ranges
|
||||
self.model.actuator_ctrllimited[body.u_e_indices] = body.ctrl_limited[body_e_indices]
|
||||
self.model.actuator_ctrlrange[body.u_e_indices, 0] = body.ctrl_range[body_e_indices, 0]
|
||||
self.model.actuator_ctrlrange[body.u_e_indices, 1] = body.ctrl_range[body_e_indices, 1]
|
||||
self.model.actuator_forcelimited[body.u_e_indices] = body.force_limited[body_e_indices]
|
||||
self.model.actuator_forcerange[body.u_e_indices, 0] = body.force_range[body_e_indices, 0]
|
||||
self.model.actuator_forcerange[body.u_e_indices, 1] = body.force_range[body_e_indices, 1]
|
||||
|
||||
if joint_ids is None:
|
||||
self.sim.data.ctrl[body.u_e_indices] = torques
|
||||
|
||||
q_idx = np.array(range(body.num_actuated_joints))
|
||||
else:
|
||||
# check if valid joints
|
||||
self._check_joint_ids(body, joint_ids)
|
||||
|
||||
# get q indices e.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True)
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = q_idx[q_idx != -1]
|
||||
|
||||
# if one joint, set its efforts (torques/forces)
|
||||
if isinstance(q_idx, int):
|
||||
if q_idx == -1:
|
||||
return
|
||||
self.sim.data.ctrl[body.u_e_indices[q_idx]] = torques
|
||||
if len(q_idx) == 0:
|
||||
raise ValueError("No actuated joints to set the torques to...")
|
||||
|
||||
# if multiple joints, set their efforts (torques/forces)
|
||||
else:
|
||||
self.sim.data.ctrl[body.u_e_indices + q_idx[q_idx != -1]] = torques
|
||||
# check the current control mode, deactivate the other motors by setting their gains and biases to zero
|
||||
if np.any(body.ctrl_modes != struct.ControlMode.EFFORT):
|
||||
# Switch to effort control mode
|
||||
body.ctrl_modes[q_idx] = struct.ControlMode.EFFORT
|
||||
body_e_indices = body.u_e_indices - body.u_idx0
|
||||
self.model.actuator_gainprm[body.u_p_indices[q_idx]] = 0
|
||||
self.model.actuator_biasprm[body.u_p_indices[q_idx]] = 0
|
||||
self.model.actuator_gainprm[body.u_v_indices[q_idx]] = 0
|
||||
self.model.actuator_biasprm[body.u_v_indices[q_idx]] = 0
|
||||
self.model.actuator_gainprm[body.u_e_indices[q_idx], 0] = body.gains[body_e_indices[q_idx], 0]
|
||||
self.model.actuator_biasprm[body.u_e_indices[q_idx]] = 0
|
||||
|
||||
# reset to original control and force ranges
|
||||
self.model.actuator_ctrllimited[body.u_e_indices[q_idx]] = body.ctrl_limited[body_e_indices[q_idx]]
|
||||
self.model.actuator_ctrlrange[body.u_e_indices[q_idx], 0] = body.ctrl_range[body_e_indices[q_idx], 0]
|
||||
self.model.actuator_ctrlrange[body.u_e_indices[q_idx], 1] = body.ctrl_range[body_e_indices[q_idx], 1]
|
||||
self.model.actuator_forcelimited[body.u_e_indices[q_idx]] = body.force_limited[body_e_indices[q_idx]]
|
||||
self.model.actuator_forcerange[body.u_e_indices[q_idx], 0] = body.force_range[body_e_indices[q_idx], 0]
|
||||
self.model.actuator_forcerange[body.u_e_indices[q_idx], 1] = body.force_range[body_e_indices[q_idx], 1]
|
||||
|
||||
# set joint efforts (torques/forces)
|
||||
self.sim.data.ctrl[body.u_e_indices[q_idx]] = torques
|
||||
|
||||
def get_joint_torques(self, body_id, joint_ids=None):
|
||||
"""
|
||||
@@ -3170,9 +3188,9 @@ class Mujoco(Simulator):
|
||||
return self.sim.data.qfrc_applied[body.v_idx1 + joint_ids]
|
||||
|
||||
# if multiple joints, return their velocities
|
||||
q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
q_idx = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints)
|
||||
torques = np.zeros(len(joint_ids))
|
||||
torques[q != -1] = self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]]
|
||||
torques[q_idx != -1] = self.sim.data.qfrc_applied[body.v_idx1 + q_idx[q_idx != -1]]
|
||||
return torques
|
||||
|
||||
def get_joint_reaction_forces(self, body_id, joint_ids=None):
|
||||
|
||||
@@ -0,0 +1,214 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
#!/usr/bin/env python
|
||||
"""Test with MuJoCo.
|
||||
"""
|
||||
|
||||
import os
|
||||
import time
|
||||
import numpy as np
|
||||
from itertools import count
|
||||
# from pyrobolearn.simulators.bullet import Bullet
|
||||
from pyrobolearn.simulators.mujoco import Mujoco
|
||||
import mujoco_py as mujoco
|
||||
|
||||
|
||||
# sim = Bullet(render=True)
|
||||
sim = Mujoco(render=True, update_dynamically=True)
|
||||
print("Gravity: {}".format(sim.get_gravity()))
|
||||
print("Timestep: {}".format(sim.get_time_step()))
|
||||
# sim.set_gravity(np.zeros(3))
|
||||
|
||||
# load floor
|
||||
# floor = sim.load_floor(dimension=20)
|
||||
|
||||
print("qpos (before loading): ", sim.sim.data.qpos)
|
||||
|
||||
# create box
|
||||
# box = sim.create_primitive_object(sim.GEOM_BOX, position=(0, 0, 2), mass=1, rgba_color=(1, 0, 0, 1))
|
||||
# sphere = sim.create_primitive_object(sim.GEOM_SPHERE, position=[0.5, 0., 1.], mass=0, radius=0.05,
|
||||
# rgba_color=(1, 0, 0, 0.5))
|
||||
# cylinder = sim.create_primitive_object(sim.GEOM_CYLINDER, position=(0, 2, 2), mass=1)
|
||||
# capsule = sim.create_primitive_object(sim.GEOM_CAPSULE, position=(0, -2, 2), mass=1, rgba_color=(0, 0, 1, 1))
|
||||
|
||||
print("qpos (after loading sphere): ", sim.sim.data.qpos)
|
||||
|
||||
# print("Sphere id: ", sphere)
|
||||
# print("Num bodies before loading robot: ", sim.num_bodies())
|
||||
|
||||
# load robot
|
||||
path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/rrbot/pendulum.urdf'
|
||||
# path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/rrbot/rrbot.urdf'
|
||||
path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/franka/franka.urdf'
|
||||
# path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/kuka/kuka_iiwa/iiwa14.urdf'
|
||||
path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/hyq2max/hyq2max.urdf'
|
||||
# path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/anymal/anymal.urdf'
|
||||
# path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/centauro/centauro_stick.urdf'
|
||||
robot_name = path.split('/')[-1].split('.')[0]
|
||||
robot = sim.load_urdf(path, position=(0, 0, 0.8), use_fixed_base=True)
|
||||
|
||||
print("qpos (after loading robot): ", sim.sim.data.qpos)
|
||||
|
||||
print("Base position: ", sim.get_base_position(robot))
|
||||
|
||||
print("Num bodies after loading robot: ", sim.num_bodies())
|
||||
|
||||
# print("Robot")
|
||||
# print("base name: ", sim.get_body_info(robot))
|
||||
# print("mass: ", sim.get_base_mass(body_id=robot))
|
||||
|
||||
# sim.remove_body(sphere)
|
||||
print(sim.sim.data.qpos)
|
||||
|
||||
# sim.step()
|
||||
model = sim.model
|
||||
mjc_sim = sim.sim
|
||||
data = mjc_sim.data
|
||||
|
||||
# The ones that appear in the following are because of the floor
|
||||
print("nbody", model.nbody - 1) # total number of links
|
||||
# print("nuser_body", model.nuser_body)
|
||||
print("njnt", model.njnt) # total number of joints
|
||||
print("nq", model.nq) # total number of generalized coordinates (=num_actuated_joints); for free joints 7
|
||||
print("nv", model.nv) # generalized velocities (nq - 1)
|
||||
print("na", model.na)
|
||||
print("nu", model.nu)
|
||||
print("qpos", data.qpos)
|
||||
print("qvel", data.qvel)
|
||||
print("act", data.act)
|
||||
# print("qpos", data.qpos, len(data.qpos)) # nqx1
|
||||
print("body_dofnum: ", model.body_dofnum)
|
||||
print("body_mass: ", model.body_mass)
|
||||
print("body_subtreemass", model.body_subtreemass)
|
||||
print("subtree_com", data.subtree_com)
|
||||
print("body_xpos: ", data.body_xpos)
|
||||
print("body pos: ", model.body_pos)
|
||||
print("body_xquat: ", data.body_xquat)
|
||||
# print("get_xpos: ", data.get_body_xpos(sim._bodies[sphere].tag_name))
|
||||
print("xfrc_applied: ", data.xfrc_applied)
|
||||
|
||||
# joints
|
||||
# print("jnt_type: ", [["free", "ball", "slide", "hinge"][idx] for idx in model.jnt_type])
|
||||
# print("jnt_qposadr: ", model.jnt_qposadr)
|
||||
|
||||
data.body_xpos[1] = np.array(range(3))
|
||||
|
||||
num_joints = sim.num_joints(robot)
|
||||
num_actuated_joints = sim.num_actuated_joints(robot)
|
||||
num_links = sim.num_links(robot)
|
||||
|
||||
joint_ids = sim.get_joint_type_ids(robot, list(range(num_joints)))
|
||||
joint_ids = np.array([i for i in range(num_joints) if joint_ids[i] != sim.JOINT_FIXED])
|
||||
|
||||
# define amplitude and angular velocity when moving the sphere
|
||||
w = 0.01/2
|
||||
r = 0.2
|
||||
|
||||
print("\nncam: ", model.ncam)
|
||||
print("cam_xpos: ", data.cam_xpos)
|
||||
print("cam_xmat: ", data.cam_xmat)
|
||||
print("cam_fovy: ", model.cam_fovy)
|
||||
print("Masses: ", sim.get_link_masses(robot))
|
||||
print("Names: ", sim.get_link_names(robot))
|
||||
print("Num links: ", num_links)
|
||||
print("Num joints: ", num_joints)
|
||||
print("Num actuated joints: ", num_actuated_joints)
|
||||
print("Contacts: ", data.contact)
|
||||
print("Sim state: ", mjc_sim.get_state())
|
||||
|
||||
print("time: ", data.time)
|
||||
|
||||
for i in range(num_joints):
|
||||
print(sim.get_joint_info(robot, i))
|
||||
|
||||
for i in range(num_links):
|
||||
print(sim.get_link_state(robot, i))
|
||||
|
||||
print("Jacobian: ", sim.calculate_jacobian(robot, num_actuated_joints))
|
||||
|
||||
data.qpos[:] = np.zeros(model.nq)
|
||||
|
||||
viewer = sim.viewer
|
||||
|
||||
print(viewer.cam)
|
||||
print(dir(viewer.cam))
|
||||
|
||||
# sim.reset_joint_states(robot, positions=[8.84305270e-05, 7.11378917e-02, -1.68059886e-04, -9.71690439e-01,
|
||||
# 1.68308810e-05, 3.71467111e-01, 5.62890805e-05])
|
||||
|
||||
print(sim.print_xml())
|
||||
|
||||
# TODO: the angles are reversed when setting qpos0
|
||||
# TODO: the robots
|
||||
if robot_name == 'franka':
|
||||
positions = np.array([0.0277854, -0.97229678, -0.028778385, -2.427800237, -0.086976557, 1.442695354, -0.711514286,
|
||||
0., 0.])
|
||||
sim.reset_joint_states(robot, joint_ids=joint_ids, positions=positions)
|
||||
elif robot_name == 'iiwa14':
|
||||
sim.reset_joint_state(robot, joint_id=3, position=-np.pi/2)
|
||||
elif robot_name == 'pendulum':
|
||||
sim.reset_joint_state(robot, joint_id=1, position=np.pi/8)
|
||||
|
||||
# perform step
|
||||
for t in count():
|
||||
# print("nbody", model.nbody)
|
||||
# print("njnt", model.njnt)
|
||||
# print("nq", model.nq)
|
||||
# print("nv", model.nv)
|
||||
# print("na", model.na)
|
||||
# print("nu", model.nu)
|
||||
# print("qpos", mjc_sim.data.qpos) # nqx1
|
||||
# print("body_dofnum: ", model.body_dofnum)
|
||||
# print("body_mass: ", model.body_mass)
|
||||
# print("body_subtreemass", model.body_subtreemass)
|
||||
# if (t % 200) == 0:
|
||||
# print("Resetting position")
|
||||
# model.body_pos[1] = range(3)
|
||||
# # data.qpos[:3] = range(3)
|
||||
# print(model.body_pos)
|
||||
# print(mjc_sim.data.body_xpos[1])
|
||||
|
||||
# if t % 200 == 0:
|
||||
# # print(mjc_sim.data.subtree_com)
|
||||
# pos = np.zeros(3)
|
||||
# jacp = np.zeros(3 * model.nv)
|
||||
# jacr = np.zeros(3 * model.nv)
|
||||
# mujoco.functions.mj_jac(model, data, jacp, jacr, pos, 4)
|
||||
# print(jacp)
|
||||
# print(jacr)
|
||||
# # model.body_pos[1] = range(3)
|
||||
# # sim.reset_base_position(sphere, [2, -1, 3])
|
||||
|
||||
# position = np.array([0.5, r * np.cos(w * t + np.pi / 2), (1. - r) + r * np.sin(w * t + np.pi / 2)])
|
||||
# sim.reset_base_position(sphere, position)
|
||||
# data.qpos[:] = np.zeros(model.nq)
|
||||
|
||||
# print joint positions
|
||||
# print(sim.get_joint_positions(robot))
|
||||
# sim.set_joint_positions(robot, joint_ids, 0 * np.ones(num_actuated_joints))
|
||||
# sim.reset_joint_states(robot, joint_ids=joint_ids, positions=positions)
|
||||
# sim.set_joint_positions(robot, joint_ids, positions)
|
||||
# sim.set_joint_positions(robot, joint_ids, [0., 0., 0., np.pi/2, 0., 0., 0.])
|
||||
|
||||
sim.set_joint_positions(robot, joint_ids, np.zeros(num_actuated_joints), kps=50, kds=1)
|
||||
# sim.set_joint_positions(robot, joint_ids, np.pi/2 * np.ones(num_actuated_joints), kps=100, kds=10)
|
||||
# sim.set_joint_positions(robot, joint_ids=1, positions=np.pi/2, kps=100, kds=10)
|
||||
# sim.set_joint_velocities(robot, joint_ids, np.zeros(num_actuated_joints))
|
||||
# sim.set_joint_velocities(robot, joint_ids, velocities=5 * np.ones(num_actuated_joints))
|
||||
# sim.set_joint_torques(robot, joint_ids, torques=np.zeros(num_actuated_joints))
|
||||
# sim.set_joint_torques(robot, joint_ids, torques=5 * np.ones(num_actuated_joints))
|
||||
|
||||
# if t == 500:
|
||||
# cylinder = sim.create_primitive_object(sim.GEOM_CYLINDER, position=(0, 2, 2), mass=1)
|
||||
|
||||
# if t == 2000:
|
||||
# sim.remove_body(cylinder)
|
||||
|
||||
# qpos = data.qpos
|
||||
# print("time: ", data.time)
|
||||
# # print(qpos)
|
||||
# data.qpos[:] = np.zeros(len(qpos))
|
||||
# print(data.xfrc_applied.shape)
|
||||
# print(data.mocap_quat)
|
||||
|
||||
# print(mjc_sim.data.contact)
|
||||
sim.step(sleep_time=sim.dt)
|
||||
Reference in New Issue
Block a user