def array_to_pose_data(self, skel, data, T_root_ref=None): assert len(data) == self._num_dofs + 6 T_root = conversions.Rp2T(conversions.A2R(data[3:6]), data[0:3]) if T_root_ref is not None: T_root = np.dot(T_root_ref, T_root) pose_data = [] idx = 6 for i in range(skel.num_joint()): joint = skel.joints[i] if joint == skel.root_joint: pose_data.append(T_root) else: j = self._char_info.bvh_map_inv[joint.name] if j is None: pose_data.append(constants.eye_T()) else: joint_type = self._joint_type[j] if joint_type == self._pb_client.JOINT_FIXED: pose_data.append(constants.eye_T()) elif joint_type == self._pb_client.JOINT_SPHERICAL: pose_data.append( conversions.R2T(conversions.A2R(data[idx:idx + 3]))) idx += 3 else: raise NotImplementedError() return pose_data
def render_cylinder_info(T, length, radius, scale=1.0, line_width=2.0, color=[0, 0, 0, 1], slice=10): glDisable(GL_LIGHTING) glPushMatrix() glTransform(T) glScalef(scale, scale, scale) glColor(color) glPushMatrix() glTranslated(0.0, 0.0, -0.5 * length) render_circle( constants.eye_T(), r=radius, slice=slice, scale=1.0, line_width=line_width, color=color, draw_plane="xy", ) glTranslated(0.0, 0.0, length) render_circle( constants.eye_T(), r=radius, slice=slice, scale=1.0, line_width=line_width, color=color, draw_plane="xy", ) glPopMatrix() render_line( p1=[radius, 0.0, -0.5 * length], p2=[radius, 0.0, 0.5 * length], color=color, line_width=line_width, ) glPopMatrix() glEnable(GL_LIGHTING)
def get_random_T(): R = get_random_R() p = np.random.rand(3) T = constants.eye_T() T[:3, :3] = R T[:3, 3] = p return T
def render_joints(pb_client, model): for j in range(pb_client.getNumJoints(model)): joint_info = pb_client.getJointInfo(model, j) joint_local_p, joint_local_Q, link_idx = joint_info[14], joint_info[ 15], joint_info[16] T_joint_local = conversions.Qp2T(np.array(joint_local_Q), np.array(joint_local_p)) if link_idx == -1: link_world_p, link_world_Q = pb_client.getBasePositionAndOrientation( model) else: link_info = pb_client.getLinkState(model, link_idx) link_world_p, link_world_Q = link_info[0], link_info[1] T_link_world = conversions.Qp2T(np.array(link_world_Q), np.array(link_world_p)) T_joint_world = np.dot(T_link_world, T_joint_local) # Render joint position glPushMatrix() gl_render.glTransform(T_joint_world) gl_render.render_point(np.zeros(3), radius=0.02, color=[0, 0, 0, 1]) # Render joint axis depending on joint types joint_type = joint_info[2] if joint_type == pb_client.JOINT_FIXED: pass elif joint_type == pb_client.JOINT_REVOLUTE: axis = joint_info[13] gl_render.render_line(np.zeros(3), axis, color=[1, 0, 0, 1], line_width=1.0) elif joint_type == pb_client.JOINT_SPHERICAL: gl_render.render_transform(constants.eye_T(), scale=0.2) else: raise NotImplementedError() glPopMatrix()
def render_body(body, option, global_coord=True): T = body.parent_bodynode.T if body.parent_bodynode else constants.eye_T() glPushAttrib(GL_LIGHTING) if option.lighting: glEnable(GL_LIGHTING) else: glDisable(GL_LIGHTING) if option.render_face: s = option.scale glPushMatrix() if global_coord: gl_render.glTransform(T) glScalef(s, s, s) body.render_with_color(option.color_face) glPopMatrix() if option.render_edge: glLineWidth(option.line_width) s = option.scale glPushMatrix() if global_coord: gl_render.glTransform(T) glScalef(s, s, s) glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) body.render_with_color(option.color_edge) glPolygonMode(GL_FRONT_AND_BACK, GL_FILL) glPopMatrix() glPopAttrib(GL_LIGHTING)
def render_ground( size=[20.0, 20.0], dsize=[1.0, 1.0], color=[0.0, 0.0, 0.0, 1.0], line_width=1.0, axis="y", origin=True, use_arrow=False, lighting=False, ): lx = size[0] lz = size[1] dx = dsize[0] dz = dsize[1] nx = int(lx / dx) + 1 nz = int(lz / dz) + 1 glColor(color) glLineWidth(line_width) if lighting: glEnable(GL_LIGHTING) if axis is "x": for i in np.linspace(-0.5 * lx, 0.5 * lx, nx): glBegin(GL_LINES) glVertex3d(0, i, -0.5 * lz) glVertex3d(0, i, 0.5 * lz) glEnd() for i in np.linspace(-0.5 * lz, 0.5 * lz, nz): glBegin(GL_LINES) glVertex3d(0, -0.5 * lx, i) glVertex3d(0, 0.5 * lx, i) glEnd() elif axis is "y": for i in np.linspace(-0.5 * lx, 0.5 * lx, nx): glBegin(GL_LINES) glVertex3d(i, 0, -0.5 * lz) glVertex3d(i, 0, 0.5 * lz) glEnd() for i in np.linspace(-0.5 * lz, 0.5 * lz, nz): glBegin(GL_LINES) glVertex3d(-0.5 * lx, 0, i) glVertex3d(0.5 * lx, 0, i) glEnd() elif axis is "z": for i in np.linspace(-0.5 * lx, 0.5 * lx, nx): glBegin(GL_LINES) glVertex3d(i, -0.5 * lz, 0) glVertex3d(i, 0.5 * lz, 0) glEnd() for i in np.linspace(-0.5 * lz, 0.5 * lz, nz): glBegin(GL_LINES) glVertex3d(-0.5 * lx, i, 0) glVertex3d(0.5 * lx, i, 0) glEnd() if origin: render_transform(constants.eye_T(), use_arrow=use_arrow)
def invertT(T): R = T[:3, :3] p = T[:3, 3] invT = constants.eye_T() R_trans = R.transpose() R_trans_p = np.dot(R_trans, p) invT[:3, :3] = R_trans invT[:3, 3] = -R_trans_p return invT
def Rp2T(R, p): input_shape = R.shape[:-2] if R.ndim > 2 else p.shape[:-1] R_flat = R.reshape((-1, 3, 3)) p_flat = p.reshape((-1, 3)) T = np.zeros((int(np.prod(input_shape)), 4, 4)) T[...] = constants.eye_T() T[..., :3, :3] = R_flat T[..., :3, 3] = p_flat return T.reshape(list(input_shape) + [4, 4])
def parse_amc(file_path, joints, skel): with open(file_path) as f: content = f.read().splitlines() for idx, line in enumerate(content): if line == ":DEGREES": content = content[idx + 1:] break motion = motion_class.Motion(skel=skel) frames = [] idx = 0 line, idx = read_line(content, idx) assert line[0].isnumeric(), line EOF = False frame = 0 translation_data = [] while not EOF: # joint_degree = {} while True: line, idx = read_line(content, idx) if line is None: EOF = True break if line[0].isnumeric(): break line_idx = 1 if "root" in line[0]: degree = np.array([float(line[i]) for i in range(4, 7)]) joints[line[0]].coordinate = np.array( [float(line[i]) for i in range(1, 4)]) else: degree = [] for lm in joints[line[0]].limits: if lm[0] != lm[1]: degree.append(float(line[line_idx])) line_idx += 1 else: degree.append(0) joints[line[0]].degree = np.deg2rad(np.array(degree).squeeze()) pose_data = [] set_rotation(joints["root"]) for key in joints.keys(): if joints[key].matrix is None: pose_data.append(constants.eye_T()) else: pose_data.append( conversions.Rp2T( joints[key].matrix.squeeze(), joints[key].coordinate.squeeze(), )) fps = 60 motion.add_one_frame(frame / fps, pose_data) frame += 1 return motion
def __init__( self, name=None, dof=3, xform_from_parent_joint=constants.eye_T(), parent_joint=None, limits=None, direction=None, length=None, axis=None, ): self.name = name if name else f"joint_{random.getrandbits(32)}" self.child_joints = [] self.index_child_joint = {} self.xform_global = constants.eye_T() self.xform_from_parent_joint = xform_from_parent_joint self.parent_joint = self.set_parent_joint(parent_joint) self.info = {"dof": dof} # set ball joint by default self.length = length if axis is not None: axis = np.deg2rad(axis) self.C = conversions.E2R(axis) self.Cinv = np.linalg.inv(self.C) self.matrix = None self.degree = np.zeros(3) self.coordinate = None if direction is not None: self.direction = direction.squeeze() if limits is not None: self.limits = np.zeros([3, 2]) for lm, nm in zip(limits, dof): if nm == "rx": self.limits[0] = lm elif nm == "ry": self.limits[1] = lm else: self.limits[2] = lm
def get_joint_position(j, p_root, Q_root, p_link, Q_link): if j == self._char_info.ROOT or self._joint_parent_link[ j] == self._char_info.ROOT: p, Q = p_root, Q_root else: p, Q = p_link[self._joint_parent_link[j]], Q_link[ self._joint_parent_link[j]] T_link_world = conversions.Qp2T(Q, p) T_joint_local = constants.eye_T( ) if j == self._char_info.ROOT else self._joint_xform_from_parent_link[ j] T_joint_world = np.dot(T_link_world, T_joint_local) return conversions.T2p(T_joint_world)
def get_pose(self, skel): p, Q = self._pb_client.getBasePositionAndOrientation(self._body_id) states = self._pb_client.getJointStatesMultiDof( self._body_id, self._joint_indices) pose_data = [] for i in range(skel.num_joint()): joint = skel.joints[i] if joint == skel.root_joint: pose_data.append(conversions.Qp2T(Q, p)) else: j = self._char_info.bvh_map_inv[joint.name] if j is None: pose_data.append(constants.eye_T()) else: joint_type = self._joint_type[j] if joint_type == self._pb_client.JOINT_FIXED: pose_data.append(constants.eye_T()) elif joint_type == self._pb_client.JOINT_SPHERICAL: pose_data.append(conversions.Q2T(states[j][0])) else: raise NotImplementedError() return motion.Pose(skel, pose_data)
def load( file, motion=None, scale=1.0, load_skel=True, load_motion=True, v_up_skel=np.array([0.0, 1.0, 0.0]), v_face_skel=np.array([0.0, 0.0, 1.0]), v_up_env=np.array([0.0, 1.0, 0.0]), ): if not motion: motion = motion_class.Motion(fps=60) if load_skel: skel = motion_class.Skeleton( v_up=v_up_skel, v_face=v_face_skel, v_up_env=v_up_env, ) smpl_offsets = np.zeros([24, 3]) smpl_offsets[0] = OFFSETS[0] for idx, pid in enumerate(SMPL_PARENTS[1:]): smpl_offsets[idx + 1] = OFFSETS[idx + 1] - OFFSETS[pid] for joint_name, parent_joint, offset in zip( SMPL_JOINTS, SMPL_PARENTS, smpl_offsets ): joint = motion_class.Joint(name=joint_name) if parent_joint == -1: parent_joint_name = None joint.info["dof"] = 6 # root joint is free offset -= offset else: parent_joint_name = SMPL_JOINTS[parent_joint] offset = offset / np.linalg.norm(smpl_offsets[4]) T1 = conversions.p2T(scale * offset) joint.xform_from_parent_joint = T1 skel.add_joint(joint, parent_joint_name) motion.skel = skel else: assert motion.skel is not None if load_motion: assert motion.skel is not None # Assume 60fps motion.fps = 60.0 dt = float(1 / motion.fps) with open(file, "rb") as f: data = pkl.load(f, encoding="latin1") poses = np.array(data["poses"]) # shape (seq_length, 135) assert len(poses) > 0, "file is empty" poses = poses.reshape((-1, len(SMPL_MAJOR_JOINTS), 3, 3)) for pose_id, pose in enumerate(poses): pose_data = [ constants.eye_T() for _ in range(len(SMPL_JOINTS)) ] major_joint_id = 0 for joint_id, joint_name in enumerate(SMPL_JOINTS): if joint_id in SMPL_MAJOR_JOINTS: pose_data[ motion.skel.get_index_joint(joint_name) ] = conversions.R2T(pose[major_joint_id]) major_joint_id += 1 motion.add_one_frame(pose_id * dt, pose_data) return motion
def actuate(self, pose=None, vel=None, torque=None): if self._actuation == SimAgent.Actuation.NONE: return joint_indices = [] target_positions = [] target_velocities = [] kps = [] kds = [] max_forces = [] for j in self._joint_indices: joint_type = self.get_joint_type(j) if joint_type == self._pb_client.JOINT_FIXED: ''' Ignore fixed joints ''' continue joint_indices.append(j) if self._actuation == SimAgent.Actuation.TQ: ''' No need to compute target values for torque control ''' continue if self._char_info.bvh_map[j] == None: ''' Use the initial pose if no mapping exists ''' target_pos = self._joint_pose_init[j] target_vel = self._joint_vel_init[j] else: ''' Convert target pose value so that it fits to each joint type For the hinge joint, we find the geodesic closest value given the axis For the ''' if pose is None: T = constants.eye_T() else: T = pose.get_transform(self._char_info.bvh_map[j], local=True) if vel is None: w = np.zeros(3) else: w = vel.get_angular(self._char_info.bvh_map[j]) if joint_type == self._pb_client.JOINT_REVOLUTE: axis = self.get_joint_axis(j) target_pos = np.array( [math.project_rotation_1D(conversions.T2R(T), axis)]) target_vel = np.array( [math.project_angular_vel_1D(w, axis)]) max_force = np.array([self._char_info.max_force[j]]) elif joint_type == self._pb_client.JOINT_SPHERICAL: Q, p = conversions.T2Qp(T) Q = quaternion.Q_op(Q, op=["normalize", "halfspace"]) target_pos = Q target_vel = w max_force = np.ones(3) * self._char_info.max_force[j] else: raise NotImplementedError target_positions.append(target_pos) target_velocities.append(target_vel) if self._actuation == SimAgent.Actuation.SPD: kps.append(self._char_info.kp[j]) kds.append(self._char_info.kd[j]) elif self._actuation == SimAgent.Actuation.PD: ''' TODO: remove ''' kps.append(1.5 * self._char_info.kp[j]) kds.append(0.01 * self._char_info.kd[j]) elif self._actuation==SimAgent.Actuation.CPD or \ self._actuation==SimAgent.Actuation.CP or \ self._actuation==SimAgent.Actuation.V: kps.append(self._char_info.cpd_ratio * self._char_info.kp[j]) kds.append(self._char_info.cpd_ratio * self._char_info.kd[j]) max_forces.append(max_force) if self._actuation == SimAgent.Actuation.SPD: self._pb_client.setJointMotorControlMultiDofArray( self._body_id, joint_indices, self._pb_client.STABLE_PD_CONTROL, targetPositions=target_positions, targetVelocities=target_velocities, forces=max_forces, positionGains=kps, velocityGains=kds) elif self._actuation == SimAgent.Actuation.PD: ''' Standard PD in Bullet does not support spherical joint yet ''' # self._pb_client.setJointMotorControlMultiDofArray(self._body_id, # joint_indices, # self._pb_client.PD_CONTROL, # targetPositions=target_positions, # targetVelocities=target_velocities, # forces=max_forces, # positionGains=kps, # velocityGains=kds) forces = bu.compute_PD_forces(pb_client=self._pb_client, body_id=self._body_id, joint_indices=joint_indices, desired_positions=target_positions, desired_velocities=target_velocities, kps=kps, kds=kds, max_forces=max_forces) self._pb_client.setJointMotorControlMultiDofArray( self._body_id, joint_indices, self._pb_client.TORQUE_CONTROL, forces=forces) elif self._actuation == SimAgent.Actuation.CPD: self._pb_client.setJointMotorControlMultiDofArray( self._body_id, joint_indices, self._pb_client.POSITION_CONTROL, targetPositions=target_positions, targetVelocities=target_velocities, forces=max_forces, positionGains=kps, velocityGains=kds) elif self._actuation == SimAgent.Actuation.CP: self._pb_client.setJointMotorControlMultiDofArray( self._body_id, joint_indices, self._pb_client.POSITION_CONTROL, targetPositions=target_positions, forces=max_forces, positionGains=kps) elif self._actuation == SimAgent.Actuation.V: self._pb_client.setJointMotorControlMultiDofArray( self._body_id, joint_indices, self._pb_client.VELOCITY_CONTROL, targetVelocities=target_velocities, forces=max_forces, velocityGains=kds) elif self._actuation == SimAgent.Actuation.TQ: self._pb_client.setJointMotorControlMultiDofArray( self._body_id, joint_indices, self._pb_client.TORQUE_CONTROL, forces=torque) else: raise NotImplementedError
def load( file, motion=None, scale=1.0, load_skel=True, load_motion=True, v_up_skel=np.array([0.0, 1.0, 0.0]), v_face_skel=np.array([0.0, 0.0, 1.0]), v_up_env=np.array([0.0, 1.0, 0.0]), ): if not motion: motion = motion_classes.Motion() words = None with open(file, "rb") as f: words = [word.decode() for line in f for word in line.split()] f.close() assert words is not None and len(words) > 0 cnt = 0 total_depth = 0 joint_stack = [None, None] joint_list = [] parent_joint_list = [] if load_skel: assert motion.skel is None motion.skel = motion_classes.Skeleton( v_up=v_up_skel, v_face=v_face_skel, v_up_env=v_up_env, ) if load_skel: while cnt < len(words): # joint_prev = joint_stack[-2] joint_cur = joint_stack[-1] word = words[cnt].lower() if word == "root" or word == "joint": parent_joint_list.append(joint_cur) name = words[cnt + 1] joint = motion_classes.Joint(name=name) joint_stack.append(joint) joint_list.append(joint) cnt += 2 elif word == "offset": x, y, z = ( float(words[cnt + 1]), float(words[cnt + 2]), float(words[cnt + 3]), ) T1 = conversions.p2T(scale * np.array([x, y, z])) joint_cur.xform_from_parent_joint = T1 cnt += 4 elif word == "channels": ndofs = int(words[cnt + 1]) if ndofs == 6: joint_cur.info["type"] = "free" elif ndofs == 3: joint_cur.info["type"] = "ball" elif ndofs == 1: joint_cur.info["type"] = "revolute" else: raise Exception("Undefined") joint_cur.info["dof"] = ndofs joint_cur.info["bvh_channels"] = [] for i in range(ndofs): joint_cur.info["bvh_channels"].append(words[cnt + 2 + i].lower()) cnt += ndofs + 2 elif word == "end": joint_dummy = motion_classes.Joint(name="END") joint_stack.append(joint_dummy) cnt += 2 elif word == "{": total_depth += 1 cnt += 1 elif word == "}": joint_stack.pop() total_depth -= 1 cnt += 1 if total_depth == 0: for i in range(len(joint_list)): motion.skel.add_joint( joint_list[i], parent_joint_list[i], ) break elif word == "hierarchy": cnt += 1 else: raise Exception(f"Unknown Token {word} at token {cnt}") if load_motion: assert motion.skel is not None assert np.allclose(motion.skel.v_up, v_up_skel) assert np.allclose(motion.skel.v_face, v_face_skel) assert np.allclose(motion.skel.v_up_env, v_up_env) while cnt < len(words): word = words[cnt].lower() if word == "motion": num_frames = int(words[cnt + 2]) dt = float(words[cnt + 5]) motion.set_fps(round(1 / dt)) cnt += 6 t = 0.0 range_num_dofs = range(motion.skel.num_dofs) positions = np.zeros( (num_frames, motion.skel.num_joints(), 3, 3)) rotations = np.zeros((num_frames, motion.skel.num_joints(), 3)) T = np.zeros((num_frames, motion.skel.num_joints(), 4, 4)) T[...] = constants.eye_T() position_channels = { "xposition": 0, "yposition": 1, "zposition": 2, } rotation_channels = { "xrotation": 0, "yrotation": 1, "zrotation": 2, } for frame_idx in range(num_frames): # if frame_idx == 1: # break raw_values = [ float(words[cnt + j]) for j in range_num_dofs ] cnt += motion.skel.num_dofs cnt_channel = 0 for joint_idx, joint in enumerate(motion.skel.joints): for channel in joint.info["bvh_channels"]: value = raw_values[cnt_channel] if channel in position_channels: value = scale * value positions[frame_idx][joint_idx][ position_channels[channel]][ position_channels[channel]] = value elif channel in rotation_channels: value = conversions.deg2rad(value) rotations[frame_idx][joint_idx][ rotation_channels[channel]] = value else: raise Exception("Unknown Channel") cnt_channel += 1 for joint_idx, joint in enumerate(motion.skel.joints): for channel in joint.info["bvh_channels"]: if channel in position_channels: T[:, joint_idx] = T[:, joint_idx] @ conversions.p2T( positions[:, joint_idx, position_channels[channel], :, ]) elif channel == "xrotation": T[:, joint_idx] = T[:, joint_idx] @ conversions.R2T( conversions.Ax2R( rotations[:, joint_idx, rotation_channels[channel], ])) elif channel == "yrotation": T[:, joint_idx] = T[:, joint_idx] @ conversions.R2T( conversions.Ay2R( rotations[:, joint_idx, rotation_channels[channel], ])) elif channel == "zrotation": T[:, joint_idx] = T[:, joint_idx] @ conversions.R2T( conversions.Az2R( rotations[:, joint_idx, rotation_channels[channel], ])) for i in range(num_frames): motion.add_one_frame(list(T[i])) t += dt else: cnt += 1 assert motion.num_frames() > 0 return motion
def render_ground_texture( tex_id, size=[20.0, 20.0], dsize=[1.0, 1.0], axis="y", origin=True, use_arrow=True, circle_cut=False, circle_color=[1, 1, 1, 1], circle_offset=0.001, ): assert tex_id > 0 lx = size[0] lz = size[1] dx = dsize[0] dz = dsize[1] nx = int(lx / dx) + 1 nz = int(lz / dz) + 1 if axis is "x": raise NotImplementedError elif axis is "y": up_vec = np.array([0.0, 1.0, 0.0]) p1 = np.array([-0.5 * size[0], 0, -0.5 * size[0]]) p2 = np.array([0.5 * size[0], 0, -0.5 * size[0]]) p3 = np.array([0.5 * size[0], 0, 0.5 * size[0]]) p4 = np.array([-0.5 * size[0], 0, 0.5 * size[0]]) elif axis is "z": up_vec = np.array([0.0, 0.0, 1.0]) p1 = np.array([-0.5 * size[0], -0.5 * size[0], 0]) p2 = np.array([0.5 * size[0], -0.5 * size[0], 0]) p3 = np.array([0.5 * size[0], 0.5 * size[0], 0]) p4 = np.array([-0.5 * size[0], 0.5 * size[0], 0]) render_quad( p1, p2, p3, p4, tex_id=tex_id, tex_param1=[0, 0], tex_param2=[size[0] / dsize[0], 0], tex_param3=[size[0] / dsize[0], size[1] / dsize[1]], tex_param4=[0, size[1] / dsize[0]], ) if origin: render_transform(constants.eye_T(), use_arrow=use_arrow) if circle_cut: r_inner = min(0.5 * size[0], 0.5 * size[1]) r_outer = 1.5 * max(0.5 * size[0], 0.5 * size[1]) offset = circle_offset * up_vec glDisable(GL_LIGHTING) glPushMatrix() glTranslatef(offset[0], offset[1], offset[2]) if axis is "y": glRotated(-90.0, 1, 0, 0) render_disk( constants.eye_T(), r_inner=r_inner, r_outer=r_outer, slice1=64, slice2=32, scale=1.0, color=circle_color, ) glPopMatrix()