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,
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,
*args,
obs_scaling=0.1,
use_rel_pos_err=False,
pos_reward=True,
rot_reward=True,
pos_vel_reward=False,
rot_vel_reward=False,
peg_tip_height_reward=True,
peg_tip_orientation_reward=True,
contact_reward=False,
use_ft_sensor=False,
random_init_pos_file=None,
reward_scale=1.0,
**kwargs):
# Store arguments.
self.use_ft_sensor = use_ft_sensor
self.obs_scaling = obs_scaling
self.use_rel_pos_err = use_rel_pos_err
self.pos_reward = pos_reward
self.rot_reward = rot_reward
self.pos_vel_reward = pos_vel_reward
self.rot_vel_reward = rot_vel_reward
self.peg_tip_height_reward = peg_tip_height_reward
self.peg_tip_orientation_reward = peg_tip_orientation_reward
self.contact_reward = contact_reward
self.reward_scale = reward_scale
# Resolve the models path based on the hole_id.
kwargs['model_path'] = kwargs.get('model_path',
'full_pushing_experiment.xml')
super(PushingEnv, self).__init__(*args, **kwargs)
# Compute good states using inverse kinematics.
if self.random_target:
raise NotImplementedError
self.kuka_pos_idx = get_qpos_indices(
self.model, ['kuka_joint_{}'.format(i) for i in range(1, 8)])
self.kuka_vel_idx = get_qvel_indices(
self.model, ['kuka_joint_{}'.format(i) for i in range(1, 8)])
self.block_pos_idx = get_qpos_indices(self.model, ['block_position'])
self.block_vel_idx = get_qvel_indices(self.model, ['block_position'])
# Set the initial conditions
if random_init_pos_file is None:
self.init_qpos_kuka = [
np.array(
[
-0.39644391, 0.90374878, 0.29834325, -1.32769707,
0.32136548, 1.0627125, -0.37159982
]
) # positioned to the -y, +x, -z, and oriented pi/6 rad toward the skinny long block
# np.array([-0.18183141, 0.82372004, 0.12119514, -1.36660597, 0.13291881, 0.97885664, -0.17394202]) # positioned to the -y, +x, -z, and oriented 0.2 rad toward the skinny long block
# np.array([-3.09971876e-02, 8.27156181e-01, -3.99097887e-04, -1.33895589e+00, 3.54769752e-04, 9.75480647e-01, -3.14663096e-02]) # positioned to the -y, +x, -z of the skinny long block
# np.array([-3.08291276e-02, 8.00410366e-01, -6.76915982e-04, -1.33038224e+00, 5.73359368e-04, 1.01080023e+00, -3.16050986e-02]), # positioned to the -y and +x of the skinny long block
# np.array([ 2.42308236, -1.01571971, 1.13996587, 1.56773622, 2.17062176, 1.20969055, 0.61193454]), # positioned to the -y and +x of the skinny block
# np.array([-0.07025654, 0.6290658, -0.00323965, -1.64794655, 0.0025054, 0.86458435, -0.07450195]), # positioned to the -y and +x of the block
# np.array([ 0.74946844, 0.98614739, 1.88508577, 1.80629075, 1.02973813, -1.18159247, 2.28928049]), # positioned to the -y of the block
# np.array([ 0.84985144, 0.97250624, 1.83905997, 1.80017142, 1.01155183, -1.2224522, 2.37542027]), # positioned above the block (bent elbow)
# np.array([-7.25614932e-06, 5.04007949e-01, 9.31413754e-06, -1.80017133e+00, -6.05474878e-06, 8.37413374e-01, 4.95278012e-06]), # positioned above the block (straight elbow)
]
else:
random_init_pos_file = os.path.join(kuka_asset_dir(),
random_init_pos_file)
self.init_qpos_kuka = np.load(random_init_pos_file)
self.init_qpos_block = np.array([.7, 0, 1.2, 1, 0, 0, 0])
self.table_height = self.init_qpos_block[2] - 0.02
# import pdb; pdb.set_trace()
# self.block_target_position = np.array([.7, .1, 1.2, 0.70710678118, 0, 0, 0.70710678118])
# self.block_target_position = np.array([.7, .1, 1.2, 0., 0., 0., 1.])
self.block_target_position = np.array(
[0.7, 0.1, 1.2, 0.92387953251, 0, 0, 0.38268343236])
self.tip_geom_id = self.model.geom_name2id('peg_tip_geom')
self.block_geom_id = self.model.geom_name2id('block_geom')
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,
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)