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 = [
