def __init__(self,
sim,
action_scaling=10.,
gravity_comp_model_path=None,
controlled_joints=None):
super(DirectTorqueController, self).__init__(sim)
self.gravity_comp = False
if gravity_comp_model_path is not None:
self.gravity_comp = True
model_path = os.path.join(kuka_asset_dir(),
gravity_comp_model_path)
self.model = mujoco_py.load_model_from_path(model_path)
self.gravity_comp_sim = mujoco_py.MjSim(self.model)
assert self.model.nv == self.sim.model.nv, \
"the model for control and simulation must have the same number of DOF"
# Get the position, velocity, and actuator indices for the model.
if controlled_joints is not None:
self.sim_qpos_idx = get_qpos_indices(sim.model,
controlled_joints)
self.sim_qvel_idx = get_qvel_indices(sim.model,
controlled_joints)
self.sim_actuators_idx = get_actuator_indices(
sim.model, controlled_joints)
self.sim_joint_idx = get_joint_indices(sim.model,
controlled_joints)
self.self_qpos_idx = get_qpos_indices(self.model,
controlled_joints)
self.self_qvel_idx = get_qvel_indices(self.model,
controlled_joints)
self.self_actuators_idx = get_actuator_indices(
self.model, controlled_joints)
else:
assert self.model.nv == self.model.nu, "if the number of degrees of freedom is different than the number of actuators you must specify the controlled_joints"
self.sim_qpos_idx = range(self.model.nq)
self.sim_qvel_idx = range(self.model.nv)
self.sim_actuators_idx = range(self.model.nu)
self.sim_joint_idx = range(self.model.nu)
self.self_qpos_idx = range(self.model.nq)
self.self_qvel_idx = range(self.model.nv)
self.self_actuators_idx = range(self.model.nu)
# Scale the actions proportionally to the subtree mass.
true_subtree_mass = kuka_subtree_mass(sim.model)
normalized_subtree_mass = true_subtree_mass / np.max(true_subtree_mass)
self.action_scaling = action_scaling * normalized_subtree_mass
# Scale the action space to the new scaling.
low = sim.model.actuator_ctrlrange[:, 0] / action_scaling
high = sim.model.actuator_ctrlrange[:, 1] / action_scaling
self.action_space = gym.spaces.Box(low, high, dtype=np.float32)
def __init__(self,
sim,
pos_scale=1.0,
rot_scale=1.0,
pos_limit=1.0,
rot_limit=1.0,
model_path='full_kuka_no_collision_no_gravity.xml',
site_name='ee_site',
stiffness=None,
damping='auto',
null_space_damping=1.0,
null_space_stiffness=1.0,
controlled_joints=None,
nominal_pos=None,
nominal_quat=None,
nominal_qpos=None):
super(FullImpedanceController, self).__init__(sim)
# Set the zero position and quaternion of the action space.
if nominal_pos is None:
self.nominal_pos = np.array([0., 0.,
0.]) #TODO: use a real position
else:
# Convert to a numpy array if necessary.
if isinstance(nominal_pos, np.ndarray):
self.nominal_pos = nominal_pos.copy()
else:
self.nominal_pos = np.array(nominal_pos)
if nominal_quat is None:
self.nominal_quat = np.array([1., 0., 0.,
0.]) # TODO: use a real quaternion
else:
# Convert to a numpy array if necessary.
if isinstance(nominal_quat, np.ndarray):
self.nominal_quat = nominal_quat.copy()
else:
self.nominal_quat = np.array(nominal_quat)
if nominal_qpos is None:
self.nominal_qpos = np.zeros(7) # TODO: use a real pose
else:
# Convert to a numpy array if necessary.
if isinstance(nominal_qpos, np.ndarray):
self.nominal_qpos = nominal_qpos.copy()
else:
self.nominal_qpos = np.array(nominal_qpos)
# Create a model for control
model_path = os.path.join(kuka_asset_dir(), model_path)
self.model = mujoco_py.load_model_from_path(model_path)
# Construct the action space.
high_pos = pos_limit * np.ones(3)
low_pos = -high_pos
high_rot = rot_limit * np.ones(3)
low_rot = -high_rot
high = np.concatenate((high_pos, high_rot))
low = np.concatenate((low_pos, low_rot))
self.action_space = spaces.Box(low, high, dtype=np.float32)
# Controller parameters.
self.scale = np.ones(6)
self.scale[:3] *= pos_scale
self.scale[3:6] *= rot_scale
self.site_name = site_name
self.pos_set = self.nominal_pos.copy()
self.quat_set = self.nominal_quat.copy()
# Default stiffness and damping.
if stiffness is None:
self.stiffness = np.array([1.0, 1.0, 1.0, 0.3, 0.3, 0.3])
else:
self.stiffness = np.ones(6) * stiffness
if damping == 'auto':
self.damping = 2 * np.sqrt(self.stiffness)
else:
self.damping = 2 * np.sqrt(self.stiffness) * damping
self.null_space_damping = null_space_damping
self.null_space_stiffness = null_space_stiffness
# Get the position, velocity, and actuator indices for the model.
if controlled_joints is not None:
self.sim_qpos_idx = get_qpos_indices(sim.model, controlled_joints)
self.sim_qvel_idx = get_qvel_indices(sim.model, controlled_joints)
self.sim_actuators_idx = get_actuator_indices(
sim.model, controlled_joints)
self.sim_joint_idx = get_joint_indices(sim.model,
controlled_joints)
self.self_qpos_idx = get_qpos_indices(self.model,
controlled_joints)
self.self_qvel_idx = get_qvel_indices(self.model,
controlled_joints)
self.self_actuators_idx = get_actuator_indices(
self.model, controlled_joints)
else:
assert self.model.nv == self.model.nu, "if the number of degrees of freedom is different than the number of actuators you must specify the controlled_joints"
self.sim_qpos_idx = range(self.model.nq)
self.sim_qvel_idx = range(self.model.nv)
self.sim_actuators_idx = range(self.model.nu)
self.sim_joint_idx = range(self.model.nu)
self.self_qpos_idx = range(self.model.nq)
self.self_qvel_idx = range(self.model.nv)
self.self_actuators_idx = range(self.model.nu)
def __init__(self,
sim,
model_path='full_kuka_no_collision_no_gravity.xml',
action_scale=1.0,
action_limit=1.0,
kp_id=1.0,
kd_id='auto',
controlled_joints=None,
set_velocity=False,
keep_finite=False):
super(InverseDynamicsController, self).__init__(sim)
# Create a model for control
model_path = os.path.join(kuka_asset_dir(), model_path)
self.model = mujoco_py.load_model_from_path(model_path)
assert self.model.nq == sim.model.nq, "the number of states in the controlled model and the simulated model must be the same"
self.set_velocity = set_velocity
# Get the position, velocity, and actuator indices for the model.
if controlled_joints is not None:
self.sim_qpos_idx = get_qpos_indices(sim.model, controlled_joints)
self.sim_qvel_idx = get_qvel_indices(sim.model, controlled_joints)
self.sim_actuators_idx = get_actuator_indices(
sim.model, controlled_joints)
self.sim_joint_idx = get_joint_indices(sim.model,
controlled_joints)
self.self_qpos_idx = get_qpos_indices(self.model,
controlled_joints)
self.self_qvel_idx = get_qvel_indices(self.model,
controlled_joints)
self.self_actuators_idx = get_actuator_indices(
self.model, controlled_joints)
else:
assert self.model.nv == self.model.nu, "if the number of degrees of freedom is different than the number of actuators you must specify the controlled_joints"
self.sim_qpos_idx = range(self.model.nq)
self.sim_qvel_idx = range(self.model.nv)
self.sim_actuators_idx = range(self.model.nu)
self.sim_joint_idx = range(self.model.nu)
self.self_qpos_idx = range(self.model.nq)
self.self_qvel_idx = range(self.model.nv)
self.self_actuators_idx = range(self.model.nu)
low = self.sim.model.jnt_range[self.sim_joint_idx, 0]
high = self.sim.model.jnt_range[self.sim_joint_idx, 1]
low[self.sim.model.jnt_limited[self.sim_joint_idx] == 0] = -np.inf
high[self.sim.model.jnt_limited[self.sim_joint_idx] == 0] = np.inf
if keep_finite:
# Don't allow infinite bounds (necessary for SAC)
low[not np.isfinite(low)] = -3.
high[not np.isfinite(high)] = 3.
low = low * action_limit
high = high * action_limit
self.action_space = spaces.Box(low, high, dtype=np.float32)
# Controller parameters.
self.action_scale = action_scale
self.kp_id = kp_id
if kd_id == 'auto':
self.kd_id = 2 * np.sqrt(self.kp_id)
else:
self.kd_id = kd_id
# Initialize setpoint.
self.sim_qpos_set = sim.data.qpos[self.sim_qpos_idx].copy()
self.sim_qvel_set = np.zeros(len(self.sim_qvel_idx))
def __init__(self,
sim,
action_scale=1.,
action_limit=1.,
controlled_joints=None,
kp=3.,
set_velocity=False,
gravity_comp_model_path=None):
super(PDController, self).__init__(sim)
self.gravity_comp = False
if gravity_comp_model_path is not None:
self.gravity_comp = True
model_path = os.path.join(kuka_asset_dir(),
gravity_comp_model_path)
self.model = mujoco_py.load_model_from_path(model_path)
self.gravity_comp_sim = mujoco_py.MjSim(self.model)
assert self.model.nv == self.sim.model.nv, \
"the model for control and simulation must have the same number of DOF"
# Get the position, velocity, and actuator indices for the model.
if controlled_joints is not None:
self.sim_qpos_idx = get_qpos_indices(sim.model, controlled_joints)
self.sim_qvel_idx = get_qvel_indices(sim.model, controlled_joints)
self.sim_actuators_idx = get_actuator_indices(
sim.model, controlled_joints)
self.sim_joint_idx = get_joint_indices(sim.model,
controlled_joints)
if hasattr(self, 'model'):
self.self_qpos_idx = get_qpos_indices(self.model,
controlled_joints)
self.self_qvel_idx = get_qvel_indices(self.model,
controlled_joints)
self.self_actuators_idx = get_actuator_indices(
self.model, controlled_joints)
else:
self.self_qpos_idx = get_qpos_indices(self.sim.model,
controlled_joints)
self.self_qvel_idx = get_qvel_indices(self.sim.model,
controlled_joints)
self.self_actuators_idx = get_actuator_indices(
self.sim.model, controlled_joints)
else:
assert self.model.nv == self.model.nu, "if the number of degrees of freedom is different than the number of actuators you must specify the controlled_joints"
self.sim_qpos_idx = range(self.sim.model.nq)
self.sim_qvel_idx = range(self.sim.model.nv)
self.sim_actuators_idx = range(self.sim.model.nu)
self.sim_joint_idx = range(self.sim.model.nu)
if hasattr(self, 'model'):
self.self_qpos_idx = range(self.model.nq)
self.self_qvel_idx = range(self.model.nv)
self.self_actuators_idx = range(self.model.nu)
else:
self.self_qpos_idx = range(self.sim.model.nq)
self.self_qvel_idx = range(self.sim.model.nv)
self.self_actuators_idx = range(self.sim.model.nu)
# # Get the controlled joints
# if controlled_joints:
# self.controlled_joints = get_qpos_indices(sim.model, controlled_joints)
# else:
# self.controlled_joints = range(sim.model.nq)
# assert sim.model.nu == len(self.controlled_joints), \
# "The number of controlled joints ({}) should match the number of actuators in the model ({})".format(
# len(self.controlled_joints), sim.model.nu)
# Scale the action space to the new scaling.
self.set_velocity = set_velocity
self.action_scale = action_scale
low_pos = sim.model.jnt_range[self.sim_joint_idx, 0] / action_scale
high_pos = sim.model.jnt_range[self.sim_joint_idx, 1] / action_scale
low_pos[self.sim.model.jnt_limited[self.sim_joint_idx] == 0] = -np.inf
high_pos[self.sim.model.jnt_limited[self.sim_joint_idx] == 0] = np.inf
if self.set_velocity:
low_vel = np.ones_like(low_pos) / action_scale
high_vel = np.ones_like(low_pos) / action_scale
low = np.concatenate([low_pos, low_vel])
high = np.concatenate([high_pos, high_vel])
else:
low = low_pos
high = high_pos
low *= action_limit
high *= action_limit
# Scale the actions proportionally to the subtree mass.
self.action_space = gym.spaces.Box(low, high, dtype=np.float32)
# set the proportional control constants, try to make the critically damped
subtree_mass = kuka_subtree_mass(sim.model)
dt = sim.model.opt.timestep
self.kp = kp * subtree_mass
self.kd = stable_critical_damping(kp, subtree_mass, dt)
self.sim = sim
# Initialize the setpoints
self.qpos_setpoint = np.zeros(sim.model.nu)
self.qvel_setpoint = np.zeros(sim.model.nu)
def __init__(self,
sim,
pos_scale=1.0,
rot_scale=0.3,
pos_limit=1.0,
rot_limit=1.0,
model_path='full_kuka_no_collision_no_gravity.xml',
site_name='ee_site',
stiffness=None,
damping='auto',
null_space_damping=1.0,
controlled_joints=None,
in_ee_frame=False):
super(ImpedanceControllerV2, self).__init__(sim)
# Create a model for control
model_path = os.path.join(kuka_asset_dir(), model_path)
self.model = mujoco_py.load_model_from_path(model_path)
self.in_ee_frame = in_ee_frame
# Construct the action space.
high_pos = pos_limit * np.ones(3)
low_pos = -high_pos
high_rot = rot_limit * np.ones(3)
low_rot = -high_rot
high = np.concatenate((high_pos, high_rot))
low = np.concatenate((low_pos, low_rot))
self.action_space = spaces.Box(low, high, dtype=np.float32)
# Controller parameters.
self.scale = np.ones(6)
self.scale[:3] *= pos_scale
self.scale[3:6] *= rot_scale
self.site_name = site_name
self.pos_set = np.zeros(3)
self.quat_set = identity_quat.copy()
if stiffness is None:
self.stiffness = np.array([1.0, 1.0, 1.0, 0.3, 0.3, 0.3])
else:
self.stiffness = np.ones(6) * stiffness
if damping == 'auto':
self.damping = 2 * np.sqrt(self.stiffness)
else:
self.damping = np.ones(6) * damping
self.null_space_damping = null_space_damping
# Get the position, velocity, and actuator indices for the model.
if controlled_joints is not None:
self.sim_qpos_idx = get_qpos_indices(sim.model, controlled_joints)
self.sim_qvel_idx = get_qvel_indices(sim.model, controlled_joints)
self.sim_actuators_idx = get_actuator_indices(
sim.model, controlled_joints)
self.sim_joint_idx = get_joint_indices(sim.model,
controlled_joints)
self.self_qpos_idx = get_qpos_indices(self.model,
controlled_joints)
self.self_qvel_idx = get_qvel_indices(self.model,
controlled_joints)
self.self_actuators_idx = get_actuator_indices(
self.model, controlled_joints)
else:
assert self.model.nv == self.model.nu, "if the number of degrees of freedom is different than the number of actuators you must specify the controlled_joints"
self.sim_qpos_idx = range(self.model.nq)
self.sim_qvel_idx = range(self.model.nv)
self.sim_actuators_idx = range(self.model.nu)
self.sim_joint_idx = range(self.model.nu)
self.self_qpos_idx = range(self.model.nq)
self.self_qvel_idx = range(self.model.nv)
self.self_actuators_idx = range(self.model.nu)