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)