def test_full_pushing_model():
model_path = os.path.join(kuka_asset_dir(), 'full_pushing_experiment.xml')
model = mujoco_py.load_model_from_path(model_path)
joint_idx = [
model.joint_name2id('kuka_joint_{}'.format(i)) for i in range(1, 8)
]
nail_idx = model.joint_name2id('block_position')
def __init__(self,
*args,
hole_id=99,
gravity=True,
obs_scaling=0.1,
sample_good_states=False,
use_ft_sensor=False,
use_rel_pos_err=False,
quadratic_cost=True,
quadratic_rot_cost=True,
regularize_pose=False,
linear_cost=False,
logarithmic_cost=False,
sparse_cost=False,
observe_joints=True,
in_peg_frame=False,
random_hole_file='random_reachable_holes_small_randomness.npy',
init_randomness=0.01,
**kwargs):
# Store arguments.
self.obs_scaling = obs_scaling
self.sample_good_states = sample_good_states
self.use_ft_sensor = use_ft_sensor
self.use_rel_pos_err = use_rel_pos_err
self.regularize_pose = regularize_pose
self.quadratic_cost = quadratic_cost
self.linear_cost = linear_cost
self.logarithmic_cost = logarithmic_cost
self.sparse_cost = sparse_cost
self.quadratic_rot_cost = quadratic_rot_cost
self.observe_joints = observe_joints
self.in_peg_frame = in_peg_frame
self.init_randomness = init_randomness
# Resolve the models path based on the hole_id.
gravity_string = '' if gravity else '_no_gravity'
if hole_id >= 0:
kwargs['model_path'] = kwargs.get('model_path', 'full_peg_insertion_experiment{}_moving_hole_id={:03d}.xml'.format(gravity_string, hole_id))
else:
kwargs['model_path'] = kwargs.get('model_path', 'full_peg_insertion_experiment_no_hole{}.xml').format(gravity_string)
super(PegInsertionEnv, self).__init__(*args, **kwargs)
self.Q_pos = np.diag([100, 100, 100])
self.Q_rot = np.diag([30, 30, 30])
if self.regularize_pose:
self.Q_pose_reg = np.eye(7)
# Compute good states using inverse kinematics.
if self.random_target:
self.reachable_holes = np.load(os.path.join(kuka_asset_dir(), random_hole_file))
self._reset_target()
else:
self.good_states = hole_insertion_samples(self.sim, range=[0.,0.06])
def test_full_hammer_model_no_collision():
model_path = os.path.join(kuka_asset_dir(), 'full_hammer_experiment_no_collision_no_gravity.xml')
model = mujoco_py.load_model_from_path(model_path)
joint_idx = [model.joint_name2id('kuka_joint_{}'.format(i)) for i in range(1,8)]
nail_idx = model.joint_name2id('nail_position')
qpos = np.zeros(8)
qvel = np.zeros(8)
qpos[joint_idx] = np.array([0, -.5, 0, -2, 0, 0, 0])
qvel[joint_idx] = np.array([0, 0, 0, 0, 0, 20, 0])
sim_mujoco(model_path, qpos, qvel)
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 test_full_pushing_model_no_gravity():
model_path = os.path.join(kuka_asset_dir(),
'full_pushing_experiment_no_gravity.xml')
model = mujoco_py.load_model_from_path(model_path)
joint_idx = [
model.joint_name2id('kuka_joint_{}'.format(i)) for i in range(1, 8)
]
nail_idx = model.joint_name2id('block_position')
qpos = np.zeros(14)
qvel = np.zeros(13)
qpos[7:14] = np.array([.7, 0, 1.2, 1, 0, 0, 0])
# qpos[7:14] = np.array([0.7, 0.1, 1.2, 0.92387953251, 0, 0, 0.38268343236])
qpos[joint_idx] = np.array([0, 0, 0, -2, 0, .9, 0])
sim_mujoco(model_path, qpos, qvel)
def __init__(self,
sim,
pos_scale=1.0,
rot_scale=0.5,
pos_limit=10.0,
rot_limit=10.0,
model_path='full_kuka_no_collision_no_gravity.xml',
stiffness=10.0,
site_name='ee_site',
controlled_joints=None):
super(ImpedanceController, 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)
# 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()
self.stiffness = np.ones(6)*stiffness
self.damping = 0.
self.controlled_joints = controlled_joints
if self.controlled_joints is not None:
self.controlled_joints = [self.model.joint_name2id(joint) for joint in self.controlled_joints]
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 test_nail_model_friction():
model_path = os.path.join(kuka_asset_dir(), 'hammer', 'full_nail_only.xml')
qpos = np.zeros(1)
qvel = -1*np.ones(1)
sim_mujoco(model_path, qpos, qvel)
def test_valve_model_friction():
model_path = os.path.join(kuka_asset_dir(), 'valve', 'full_valve_only.xml')
qpos = np.zeros(1)
qvel = 100 * np.ones(1)
sim_mujoco(model_path, qpos, qvel)
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 test_block_model():
model_path = os.path.join(kuka_asset_dir(), 'pushing',
'full_block_only.xml')
render_mujoco(model_path)
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)
def test_full_valve_model():
model_path = os.path.join(kuka_asset_dir(),
'full_valve_experiment_no_gravity.xml')
render_mujoco(model_path)
from gym_kuka_mujoco.utils.kinematics import forwardKin, forwardKinJacobian, forwardKinSite
from gym_kuka_mujoco.utils.quaternion import mat2Quat
from gym_kuka_mujoco.envs.assets import kuka_asset_dir
import os
import mujoco_py
import numpy as np
# Get the model path
model_filename = 'full_kuka_no_collision.xml'
model_path = os.path.join(kuka_asset_dir(), model_filename)
# Construct the model and simulation objects.
model = mujoco_py.load_model_from_path(model_path)
sim = mujoco_py.MjSim(model)
# The points to be transformed.
pos = np.array([0, 0, 0], dtype=np.float64)
quat = np.array([1, 0, 0, 0], dtype=np.float64)
body_id = model.body_name2id('kuka_link_7')
# Compute the forward kinematics
xpos, xrot = forwardKin(sim, pos, quat, body_id)
jacp, jacr = forwardKinJacobian(sim, pos, body_id)
print("Position:\n{}\nRotation:\n{}".format(xpos, xrot))
print("Position Jacobian:\n{}\nRotation Jacobian:\n{}".format(jacp.T, jacr.T))
## The peg tip position at the home position.
model_filename = 'full_peg_insertion_experiment_no_hole_no_gravity.xml'
model_path = os.path.join(kuka_asset_dir(), model_filename)
world_pos,
world_quat,
peg_body_id,
qpos_idx=qpos_idx,
raise_on_fail=True)
except RuntimeError as e:
continue
poses.append(q_opt)
return poses
if __name__ == "__main__":
model_filename = 'full_pushing_experiment_no_gravity.xml'
model_path = os.path.join(kuka_asset_dir(), model_filename)
# Load path
model = mujoco_py.load_model_from_path(model_path)
sim = mujoco_py.MjSim(model)
sim.data.ctrl[:] = np.zeros(model.nu)
# Compute the solution
qpos_idx = range(7)
# poses = gen_random_pushing_poses(sim, qpos_idx, N=1000)
poses = gen_random_pushing_poses(sim, qpos_idx, N=1000)
# Visualize the solution
print("Found {} poses".format(len(poses)))
# Save.
save_path = os.path.join(kuka_asset_dir(), 'random_pushing_poses')
def test_nail_model():
model_path = os.path.join(kuka_asset_dir(), 'hammer', 'full_nail_only.xml')
render_mujoco(model_path)
def test_valve_model():
model_path = os.path.join(kuka_asset_dir(), 'valve', 'full_valve_only.xml')
render_mujoco(model_path)
