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
Exemple #2
0
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)
Exemple #3
0
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)
Exemple #6
0
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)
Exemple #7
0
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
Exemple #8
0
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])
Exemple #9
0
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
Exemple #10
0
    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)
Exemple #13
0
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
Exemple #15
0
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
Exemple #16
0
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()