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)