def _calc_ref_pose_warmup(self):
"""Calculate default reference pose during warmup period."""
motion = self.get_active_motion()
pose0 = motion.calc_frame(0)
warmup_pose = self._default_pose.copy()
pose_root_rot = motion.get_frame_root_rot(pose0)
default_root_rot = motion.get_frame_root_rot(warmup_pose)
default_root_pos = motion.get_frame_root_pos(warmup_pose)
pose_heading = motion_util.calc_heading(pose_root_rot)
default_heading = motion_util.calc_heading(default_root_rot)
delta_heading = pose_heading - default_heading
delta_heading_rot = transformations.quaternion_about_axis(
delta_heading, [0, 0, 1])
default_root_pos = pose3d.QuaternionRotatePoint(
default_root_pos, delta_heading_rot)
default_root_rot = transformations.quaternion_multiply(
delta_heading_rot, default_root_rot)
motion.set_frame_root_pos(default_root_pos, warmup_pose)
motion.set_frame_root_rot(default_root_rot, warmup_pose)
return warmup_pose
def _calc_cycle_offset_pos(self, num_cycles):
"""Calculates change in the root position after a given number of cycles.
Args:
num_cycles: Number of cycles since the start of the motion.
Returns:
Net translation of the root position.
"""
if not self._enable_cycle_offset_pos:
cycle_offset_pos = np.zeros(3)
else:
if not self._enable_cycle_offset_rot:
cycle_offset_pos = num_cycles * self._cycle_delta_pos
else:
cycle_offset_pos = np.zeros(3)
for i in range(num_cycles):
curr_heading = i * self._cycle_delta_heading
rot = transformations.quaternion_about_axis(curr_heading, [0, 0, 1])
curr_offset = pose3d.QuaternionRotatePoint(self._cycle_delta_pos, rot)
cycle_offset_pos += curr_offset
return cycle_offset_pos
def calc_heading_rot(q):
"""Return a quaternion representing the heading rotation of q along the vertical axis (z axis).
Args:
q: A quaternion that the heading is to be computed from.
Returns:
A quaternion representing the rotation about the z axis.
"""
heading = calc_heading(q)
q_heading = transformations.quaternion_about_axis(heading, [0, 0, 1])
return q_heading
def build_target_obs(self):
"""Constructs the target observations, consisting of a sequence of
target frames for future timesteps. The tartet poses to include is
specified by self._tar_frame_steps.
Returns:
An array containing the target frames.
"""
tar_poses = []
time0 = self._get_motion_time()
dt = self._env.env_time_step
motion = self.get_active_motion()
robot = self._env.robot
ref_base_pos = self._get_ref_base_position()
sim_base_rot = np.array(robot.GetBaseOrientation())
heading = motion_util.calc_heading(sim_base_rot)
if self._tar_obs_noise is not None:
heading += self._randn(0, self._tar_obs_noise[0])
inv_heading_rot = transformations.quaternion_about_axis(
-heading, [0, 0, 1])
for step in self._tar_frame_steps:
tar_time = time0 + step * dt
tar_pose = self._calc_ref_pose(tar_time)
tar_root_pos = motion.get_frame_root_pos(tar_pose)
tar_root_rot = motion.get_frame_root_rot(tar_pose)
tar_root_pos -= ref_base_pos
tar_root_pos = pose3d.QuaternionRotatePoint(
tar_root_pos, inv_heading_rot)
tar_root_rot = transformations.quaternion_multiply(
inv_heading_rot, tar_root_rot)
tar_root_rot = motion_util.standardize_quaternion(tar_root_rot)
motion.set_frame_root_pos(tar_root_pos, tar_pose)
motion.set_frame_root_rot(tar_root_rot, tar_pose)
tar_poses.append(tar_pose)
tar_obs = np.concatenate(tar_poses, axis=-1)
return tar_obs
def _apply_state_perturb(self, pose, vel):
"""Apply random perturbations to the state pose and velocities."""
root_pos_std = 0.025
root_rot_std = 0.025 * np.pi
joint_pose_std = 0.05 * np.pi
root_vel_std = 0.1
root_ang_vel_std = 0.05 * np.pi
joint_vel_std = 0.05 * np.pi
perturb_pose = pose.copy()
perturb_vel = vel.copy()
motion = self.get_active_motion()
root_pos = motion.get_frame_root_pos(perturb_pose)
root_rot = motion.get_frame_root_rot(perturb_pose)
joint_pose = motion.get_frame_joints(perturb_pose)
root_vel = motion.get_frame_root_vel(perturb_vel)
root_ang_vel = motion.get_frame_root_ang_vel(perturb_vel)
joint_vel = motion.get_frame_joints_vel(perturb_vel)
root_pos[0] += self._randn(0, root_pos_std)
root_pos[1] += self._randn(0, root_pos_std)
rand_axis = self._rand_uniform(-1, 1, size=[3])
rand_axis /= np.linalg.norm(rand_axis)
rand_theta = self._randn(0, root_rot_std)
rand_rot = transformations.quaternion_about_axis(rand_theta, rand_axis)
root_rot = transformations.quaternion_multiply(rand_rot, root_rot)
joint_pose += self._randn(0, joint_pose_std, size=joint_pose.shape)
root_vel[0] += self._randn(0, root_vel_std)
root_vel[1] += self._randn(0, root_vel_std)
root_ang_vel += self._randn(0,
root_ang_vel_std,
size=root_ang_vel.shape)
joint_vel += self._randn(0, joint_vel_std, size=joint_vel.shape)
motion.set_frame_root_pos(root_pos, perturb_pose)
motion.set_frame_root_rot(root_rot, perturb_pose)
motion.set_frame_joints(joint_pose, perturb_pose)
motion.set_frame_root_vel(root_vel, perturb_vel)
motion.set_frame_root_ang_vel(root_ang_vel, perturb_vel)
motion.set_frame_joints_vel(joint_vel, perturb_vel)
return perturb_pose, perturb_vel
def _reset_ref_motion(self):
"""Reset reference motion.
First randomly select a new reference motion from
the set of available motions, and then resets to a random point along the
selected motion.
"""
self._active_motion_id = self._sample_ref_motion()
self._origin_offset_rot = np.array([0, 0, 0, 1])
self._origin_offset_pos.fill(0.0)
self._reset_motion_time_offset()
motion = self.get_active_motion()
time = self._get_motion_time()
ref_pose = self._calc_ref_pose(time)
ref_root_pos = motion.get_frame_root_pos(ref_pose)
ref_root_rot = motion.get_frame_root_rot(ref_pose)
sim_root_pos = self._get_sim_base_position()
sim_root_rot = self._get_sim_base_rotation()
# move the root to the same position and rotation as simulated robot
self._origin_offset_pos = sim_root_pos - ref_root_pos
self._origin_offset_pos[2] = 0
ref_heading = motion_util.calc_heading(ref_root_rot)
sim_heading = motion_util.calc_heading(sim_root_rot)
delta_heading = sim_heading - ref_heading
self._origin_offset_rot = transformations.quaternion_about_axis(
delta_heading, [0, 0, 1])
self._ref_pose = self._calc_ref_pose(time)
self._ref_vel = self._calc_ref_vel(time)
self._update_ref_model()
self._prev_motion_phase = motion.calc_phase(time)
self._reset_clip_change_time()
return
def _sync_ref_origin(self, sync_root_position, sync_root_rotation):
"""Moves the origin of the reference motion, such that the root of the
simulated and reference characters are at the same location. This is used
to periodically synchronize the reference motion with the simulated
character in order to mitigate drift.
Args:
sync_root_position: boolean indicating if the root position should be
synchronized
sync_root_rotation: boolean indicating if the root rotation should be
synchronized
"""
time = self._get_motion_time()
motion = self.get_active_motion()
ref_pose = self._calc_ref_pose(time, apply_origin_offset=False)
if sync_root_rotation:
ref_rot = motion.get_frame_root_rot(ref_pose)
sim_rot = self._get_sim_base_rotation()
ref_heading = self._calc_heading(ref_rot)
sim_heading = self._calc_heading(sim_rot)
heading_diff = sim_heading - ref_heading
self._origin_offset_rot = transformations.quaternion_about_axis(
heading_diff, [0, 0, 1])
if sync_root_position:
ref_pos = motion.get_frame_root_pos(ref_pose)
ref_pos = pose3d.QuaternionRotatePoint(ref_pos,
self._origin_offset_rot)
sim_pos = self._get_sim_base_position()
self._origin_offset_pos = sim_pos - ref_pos
self._origin_offset_pos[2] = 0 # only sync along horizontal plane
return
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)
