def _GetDefaultInitOrientation(self): # The Laikago URDF assumes the initial pose of heading towards z axis, # and belly towards y axis. The following transformation is to transform # the Laikago initial orientation to our commonly used orientation: heading # towards -x direction, and z axis is the up direction. if self._urdf_filename == URDF_WITH_TOES: return [0, 0, 0, 1] else: return transformations.quaternion_from_euler( ai=math.pi / 2.0, aj=0, ak=math.pi / 2.0, axes="sxyz")
def _calc_cycle_offset_rot(self, num_cycles): """Calculates change in the root rotation after a given number of cycles. Args: num_cycles: Number of cycles since the start of the motion. Returns: Net rotation of the root orientation. """ if not self._enable_cycle_offset_rot: cycle_offset_rot = np.array([0, 0, 0, 1]) else: heading_offset = num_cycles * self._cycle_delta_heading cycle_offset_rot = transformations.quaternion_from_euler( 0, 0, heading_offset) return cycle_offset_rot
def get_params_from_action(self, action): #parse action vector to a dict with more interpretable keys ctrl_params = { 'desired_object_pos': action[:3], 'desired_object_rot': trans.quaternion_from_euler(action[3], action[4], action[5], axes='sxyz'), 'object_stiffness_trans': np.diag(action[6:9]), 'object_stiffness_rot': np.diag(action[9:12]), 'agent_link_stiffness': [ np.diag(action[(12 + i * 3):(12 + (i + 1) * 3)]) for i in range(self.n_agents) ] } return ctrl_params
from motion_imitation.utilities import motion_util # import retarget_config_a1 as config import retarget_config_laikago as config # import retarget_config_vision60 as config POS_SIZE = 3 ROT_SIZE = 4 DEFAULT_ROT = np.array([0, 0, 0, 1]) FORWARD_DIR = np.array([1, 0, 0]) GROUND_URDF_FILENAME = "plane_implicit.urdf" # reference motion FRAME_DURATION = 0.01667 REF_COORD_ROT = transformations.quaternion_from_euler(0.5 * np.pi, 0, 0) REF_POS_OFFSET = np.array([0, 0, 0]) REF_ROOT_ROT = transformations.quaternion_from_euler(0, 0, 0.47 * np.pi) REF_PELVIS_JOINT_ID = 0 REF_NECK_JOINT_ID = 3 REF_HIP_JOINT_IDS = [6, 16, 11, 20] REF_TOE_JOINT_IDS = [10, 19, 15, 23] mocap_motions = [ ["pace", "data/dog_walk00_joint_pos.txt", 162, 201], ["trot", "data/dog_walk03_joint_pos.txt", 448, 481], ["trot2", "data/dog_run04_joint_pos.txt", 630, 663], ["canter", "data/dog_run00_joint_pos.txt", 430, 459], ["left turn0", "data/dog_walk09_joint_pos.txt", 1085, 1124], ["right turn0", "data/dog_walk09_joint_pos.txt", 2404, 2450],
def reset(self): self.step_counter = 0 self.sim.resetSimulation() if self._args.viz: self.sim.configureDebugVisualizer( self.sim.COV_ENABLE_RENDERING, 0) # we will enable rendering after we loaded everything self.sim.resetDebugVisualizerCamera( cameraDistance=self._cam_dist, cameraYaw=self._cam_yaw, cameraPitch=self._cam_pitch, cameraTargetPosition=self._cam_target_pos) self.sim.setAdditionalSearchPath(pybullet_data.getDataPath()) self.sim.setGravity(self._gravity[0], self._gravity[1], self._gravity[2]) planeUid = self.sim.loadURDF("plane.urdf", basePosition=[0, 0, 0]) if self._args.agent_model == 'anchor': rest_poses = [0, 0, 0, 0, 0, 0, 0] radius = 0.5 self.nJointsPerArm = None base_height = 0.8 else: rest_poses = [0, -0.2, 0, -1.2, 0, 0, 0] radius = 0.8 base_height = 0.0 self.agentUIDs = [] angle = 0 for i in range(self.n_agents): pos = [radius * np.cos(angle), radius * np.sin(angle), base_height] if self._args.agent_model == 'anchor': uid = self.sim.loadURDF("sphere_small.urdf", useFixedBase=False, basePosition=pos) else: ori = trans.quaternion_about_axis(angle, [0, 0, 1]) ori = trans.quaternion_multiply( ori, trans.quaternion_about_axis(np.pi, [0, 0, 1])) uid = self.sim.loadURDF("kuka_iiwa/model.urdf", useFixedBase=True, basePosition=pos, baseOrientation=ori) angle += np.radians(360.0 / self.n_agents) self.agentUIDs.append(uid) if self._args.agent_model != 'anchor': self.nJointsPerArm = self.sim.getNumJoints(self.agentUIDs[0]) for id in self.agentUIDs: for i in range(self.nJointsPerArm): self.sim.resetJointState(id, i, rest_poses[i]) #we need to first set force limit to zero to use torque control!! #see: https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_dynamics.py self.sim.setJointMotorControlArray(id, range(self.nJointsPerArm), self.sim.VELOCITY_CONTROL, forces=np.zeros( self.nJointsPerArm)) #create a base baseUid = self.sim.loadURDF("table_square/table_square.urdf", useFixedBase=True) #create an object # state_object= [np.random.uniform(-0.1,0.1) # ,np.random.uniform(-0.1,0.1) # ,1.0] + list(trans.quaternion_from_euler(np.pi/2, 0, 0, axes='sxyz')) init_pos = self._args.object_init_pos init_rot = self._args.object_init_rot state_object = init_pos + list( trans.quaternion_from_euler( init_rot[0], init_rot[1], init_rot[2], axes='sxyz')) if self._args.object_deform: # self.objectUid = load_deform_object_mss( # self.sim, # self._args.object_file, # None, # self._args.object_scale, # self._args.object_mass, # state_object[:3], # state_object[3:], # 10, # bending_stiffness, # 0.01, # damping_stiffness, # 100, # elastic_stiffness, # 0.5, # friction # 0 # debug flag # ) self.objectUid = load_deform_object_nhk( self.sim, self._args.object_file, None, self._args.object_scale, self._args.object_mass, state_object[:3], state_object[3:], 180, # neohookean mu 60, # neohookean lambda 0.01, # neohookean damping 3.0, # friction 0 # debug flag ) else: self.objectUid = load_rigid_object( self.sim, self._args.object_file, self._args.object_scale, self._args.object_mass, state_object[:3], state_object[3:], texture_file=None, # rgba_color=[0, 0, 1, 0.8] ) self.sim.changeDynamics(self.objectUid, -1, lateralFriction=3.0, spinningFriction=0.8, rollingFriction=0.8) obs = self.get_obs() if self._args.viz: self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 1) return obs.astype(np.float32)
import numpy as np from motion_imitation.utilities import pose3d from pybullet_utils import transformations URDF_FILENAME = "laikago/laikago_toes.urdf" REF_POS_SCALE = 1 INIT_POS = np.array([0, 0, 0]) INIT_ROT = transformations.quaternion_from_euler(ai=np.pi / 2.0, aj=0, ak=np.pi / 2.0, axes="sxyz") SIM_TOE_JOINT_IDS = [ 7, # left hand 15, # left foot 3, # right hand 11 # right foot ] SIM_HIP_JOINT_IDS = [4, 12, 0, 8] SIM_ROOT_OFFSET = np.array([0, 0, 0]) SIM_TOE_OFFSET_LOCAL = [ np.array([-0.02, 0.0, 0.0]), np.array([-0.02, 0.0, 0.01]), np.array([-0.02, 0.0, 0.0]), np.array([-0.02, 0.0, 0.01]) ] DEFAULT_JOINT_POSE = np.array( [0, 0.67, -1.25, 0, 0.67, -1.25, 0, 0.67, -1.25, 0, 0.67, -1.25]) JOINT_DAMPING = [