def throw(self, pos_target, num=1, duration=2.0, shape=Shape.BOX, vel=8.0, r_out=2.0, r_in=0.2, mass=2.0, size=0.2 * np.ones(3), h_min=0.5): assert r_out > r_in for _ in range(num): d_out = math.random_unit_vector() d_in = math.random_unit_vector() p_from = pos_target + r_out * d_out p_projected_h = math.projectionOnVector(p_from, self.v_up_env) h_cliped = max(np.linalg.norm(p_projected_h), h_min) p_from = (p_from - p_projected_h) + h_cliped * self.v_up_env p_to = pos_target + r_in * d_in v_dir = p_to - p_from v_dir = v_dir / np.linalg.norm(v_dir) p = p_from Q = conversions.A2Q(math.random_unit_vector() * np.random.uniform(-np.pi, np.pi)) v = vel * v_dir w = np.zeros(3) obs = Obstacle("", duration, shape, mass, size, p, Q, v, w) self.launch(obs)
def append_and_blend(motion1, motion2, blend_length=0): assert isinstance(motion1, motion_class.Motion) assert isinstance(motion2, motion_class.Motion) assert motion1.skel.num_joints() == motion2.skel.num_joints() combined_motion = copy.deepcopy(motion1) combined_motion.name = f"{motion1.name}+{motion2.name}" if motion1.num_frames() == 0: for i in range(motion2.num_frames()): combined_motion.poses.append(motion2.poses[i]) if hasattr(motion1, "velocities"): combined_motion.velocities.append(motion2.velocities[i]) return combined_motion frame_target = motion2.time_to_frame(blend_length) frame_source = motion1.time_to_frame(motion1.length() - blend_length) # Translate and rotate motion2 to location of frame_source pose1 = motion1.get_pose_by_frame(frame_source) pose2 = motion2.get_pose_by_frame(0) R1, p1 = conversions.T2Rp(pose1.get_root_transform()) R2, p2 = conversions.T2Rp(pose2.get_root_transform()) # Translation to be applied dp = p1 - p2 dp = dp - math.projectionOnVector(dp, motion1.skel.v_up_env) axis = motion1.skel.v_up_env # Rotation to be applied Q1 = conversions.R2Q(R1) Q2 = conversions.R2Q(R2) _, theta = quaternion.Q_closest(Q1, Q2, axis) dR = conversions.A2R(axis * theta) motion2 = translate(motion2, dp) motion2 = rotate(motion2, dR) t_total = motion1.fps * frame_source t_processed = 0.0 poses_new = [] for i in range(motion2.num_frames()): dt = 1 / motion2.fps t_total += dt t_processed += dt pose_target = motion2.get_pose_by_frame(i) # Blend pose for a moment if t_processed <= blend_length: alpha = t_processed / float(blend_length) pose_source = motion1.get_pose_by_time(t_total) pose_target = blend(pose_source, pose_target, alpha) poses_new.append(pose_target) del combined_motion.poses[frame_source + 1:] for i in range(len(poses_new)): combined_motion.add_one_frame(0, copy.deepcopy(poses_new[i].data)) return combined_motion
def get_facing_direction_position(self, ground_height=0.0): R, p = conversions.T2Rp(self.get_root_transform()) d = np.dot(R, self._char_info.v_face) if np.allclose(d, self._char_info.v_up_env): msg = \ '\n+++++++++++++++++WARNING+++++++++++++++++++\n'+\ 'The facing direction is ill-defined ' +\ '(i.e. parellel to the world up-vector).\n'+\ 'A random direction will be assigned for the direction\n'+\ 'Be careful if your system is sensitive to the facing direction\n'+\ '+++++++++++++++++++++++++++++++++++++++++++\n' warnings.warn(msg) d = math.random_unit_vector() d = d - math.projectionOnVector(d, self._char_info.v_up_env) p = p - math.projectionOnVector(p, self._char_info.v_up_env) if ground_height != 0.0: p += ground_height * self._char_info.v_up_env return d / np.linalg.norm(d), p
def get_facing_direction_position(self): R, p = conversions.T2Rp(self.get_root_transform()) d = np.dot(R, self.skel.v_face) d = d - math.projectionOnVector(d, self.skel.v_up_env) p = p - math.projectionOnVector(p, self.skel.v_up_env) return d / np.linalg.norm(d), p
def render(self, rm, ground_height=0.0): colors = rm.COLORS_FOR_AGENTS rm.gl.glEnable(rm.gl.GL_LIGHTING) rm.gl.glEnable(rm.gl.GL_BLEND) rm.gl.glBlendFunc(rm.gl.GL_SRC_ALPHA, rm.gl.GL_ONE_MINUS_SRC_ALPHA) for i in range(self._num_agent): sim_agent = self._agent[i] char_info = self._char_info[i] if rm.flag['sim_model']: rm.gl.glEnable(rm.gl.GL_DEPTH_TEST) if rm.flag['shadow']: rm.gl.glPushMatrix() d = np.array([1, 1, 1]) d = d - math.projectionOnVector(d, char_info.v_up_env) offset = (0.001 + ground_height) * char_info.v_up_env rm.gl.glTranslatef(offset[0], offset[1], offset[2]) rm.gl.glScalef(d[0], d[1], d[2]) rm.bullet_render.render_model(self._pb_client, sim_agent._body_id, draw_link=True, draw_link_info=False, draw_joint=False, draw_joint_geom=False, ee_indices=None, color=[0.5, 0.5, 0.5, 1.0], lighting=False) rm.gl.glPopMatrix() rm.bullet_render.render_model( self._pb_client, sim_agent._body_id, draw_link=True, draw_link_info=True, draw_joint=rm.flag['joint'], draw_joint_geom=True, ee_indices=char_info.end_effector_indices, color=colors[i]) if rm.flag['collision'] and self._elapsed_time > 0.0: rm.gl.glPushAttrib(rm.gl.GL_LIGHTING | rm.gl.GL_DEPTH_TEST | rm.gl.GL_BLEND) rm.gl.glEnable(rm.gl.GL_BLEND) rm.bullet_render.render_contacts(self._pb_client, sim_agent._body_id) rm.gl.glPopAttrib() if rm.flag['com_vel']: p, Q, v, w = sim_agent.get_root_state() p, v = sim_agent.get_com_and_com_vel() rm.gl_render.render_arrow(p, p + v, D=0.01, color=[0, 0, 0, 1]) if rm.flag['facing_frame']: rm.gl.glPushAttrib(rm.gl.GL_LIGHTING | rm.gl.GL_DEPTH_TEST | rm.gl.GL_BLEND) rm.gl.glEnable(rm.gl.GL_BLEND) rm.gl_render.render_transform( sim_agent.get_facing_transform(ground_height), scale=0.5, use_arrow=True) rm.gl.glPopAttrib() if rm.flag['obstacle']: self._obs_manager.render()
def root_ee_similarity( pose1, pose2, vel1=None, vel2=None, w_root_pos=1.0, w_root_vel=1.0, w_ee_pos=1.0, w_ee_vel=1.0, T_ref_1=None, T_ref_2=None, auto_weight=True, auto_weight_sigma=0.02, ): """ This computes similarity between end_effectors and root for two poses Parameters ---------- """ assert pose1.skel.num_end_effectors() == pose2.skel.num_end_effectors() if w_root_vel > 0.0 or w_ee_vel > 0.0: assert vel1 is not None and vel2 is not None assert vel1.skel.num_end_effectors() == vel2.skel.num_end_effectors() skel = pose1.skel diff_root_pos = 0.0 diff_root_vel = 0.0 diff_ee_pos = 0.0 diff_ee_vel = 0.0 """ Differencse will be computed w.r.t. its own facing frame if the reference frame is not given """ if T_ref_1 is None: R_face_1, p_face_1 = conversions.T2Rp(pose1.get_facing_transform()) else: R_face_1, p_face_1 = conversions.T2Rp(T_ref_1) if T_ref_2 is None: R_face_2, p_face_2 = conversions.T2Rp(pose2.get_facing_transform()) else: R_face_2, p_face_2 = conversions.T2Rp(T_ref_2) R_face_1_inv = R_face_1.transpose() R_face_2_inv = R_face_2.transpose() R_root_1, p_root_1 = conversions.T2Rp(pose1.get_root_transform()) R_root_2, p_root_2 = conversions.T2Rp(pose2.get_root_transform()) if w_root_pos > 0.0: p_root_1_local = np.dot(R_face_1_inv, p_root_1 - p_face_1) p_root_2_local = np.dot(R_face_2_inv, p_root_2 - p_face_2) diff_root_pos = p_root_2_local - p_root_1_local diff_root_pos = np.dot(diff_root_pos, diff_root_pos) """ Root Velocity Difference w.r.t. Facing Frame """ if w_root_vel > 0.0: v_root_1 = vel1.get_linear(skel.root_joint, False, R_root_1) v_root_2 = vel2.get_linear(skel.root_joint, False, R_root_2) v_root_1_local = np.dot(R_face_1_inv, v_root_1) v_root_2_local = np.dot(R_face_2_inv, v_root_2) diff_root_vel = v_root_2_local - v_root_1_local diff_root_vel = np.dot(diff_root_vel, diff_root_vel) """ End Effector Position and Velocity Differences w.r.t. Facing Frame """ num_ee = skel.num_end_effectors() if num_ee > 0: if w_ee_pos > 0.0 or w_ee_vel > 0.0: R1s, p1s = [], [] R2s, p2s = [], [] ee_weights = [] for j in skel.end_effectors: R1, p1 = conversions.T2Rp(pose1.get_transform(j, local=False)) R2, p2 = conversions.T2Rp(pose2.get_transform(j, local=False)) R1s.append(R1) R2s.append(R2) p1s.append(p1) p2s.append(p2) if auto_weight: h = math_ops.projectionOnVector(p1, skel.v_up_env) ee_weights.append( math.exp(-np.dot(h, h) / auto_weight_sigma)) else: ee_weights.append(1.0) ee_weights_sum = np.sum(ee_weights) ee_weights = [w / ee_weights_sum for w in ee_weights] # print('--------------------') # for j in range(len(skel.end_effectors)): # print(skel.end_effectors[j].name, ee_weights[j]) # print('--------------------') if w_ee_pos > 0.0: for j in range(len(skel.end_effectors)): p1_local = np.dot(R_face_1_inv, p1s[j] - p_face_1) p2_local = np.dot(R_face_2_inv, p2s[j] - p_face_2) dp = p2_local - p1_local diff_ee_pos += np.dot(dp, dp) if w_ee_vel > 0.0: for j in range(len(skel.end_effectors)): v1 = vel1.get_linear(j, False, R1s[j]) v2 = vel2.get_linear(j, False, R2s[j]) v1_local = np.dot(R_face_1_inv, v1) v2_local = np.dot(R_face_2_inv, v2) dv = v2_local - v1_local diff_ee_vel += np.dot(dv, dv) # diff_ee_pos /= float(num_ee) # diff_ee_vel /= float(num_ee) diff = (w_root_pos * diff_root_pos + w_root_vel * diff_root_vel + w_ee_pos * diff_ee_pos + w_ee_vel * diff_ee_vel) return diff
def project_to_ground(self, v): return v - math.projectionOnVector(v, self._char_info.v_up_env)
def get_root_height_from_ground(self, ground_height=0.0): p, _, _, _ = bu.get_base_pQvw(self._pb_client, self._body_id) vec_root_from_ground = math.projectionOnVector( p, self._char_info.v_up_env) return np.linalg.norm(vec_root_from_ground) - ground_height