skrobot foward kinematics for end coords

[HiroIshida]

Maybe used later

def endcoord_forward_kinematics(av_seq, rot_also=True):
        # NOTE !!!
        # this function will not be used in the current implementation
        # may be uesd in MPC like controler

        def quaternion_kinematic_matrix(q):
            # dq/dt = 0.5 * mat * omega 
            q1, q2, q3, q4 = q
            mat = np.array([
                [-q2, -q3, -q4], [q1, q4, -q3], [-q4, q1, q2], [q3, -q2, q1],
                ])
            return mat * 0.5

        poses = []
        jacobs = []
        for av in av_seq:
            J_geometric = compute_jacobian_wrt_baselink(av, end_effector_cascaded_coords, rot_also=rot_also)
            J_geo_pos = J_geometric[:3]

            pos = end_effector_cascaded_coords.worldpos()
            if rot_also:
                rot = end_effector_cascaded_coords.worldcoords().quaternion
                pose = np.hstack((pos, rot))

                kine_mat = quaternion_kinematic_matrix(rot)
                J_geo_rot = J_geometric[3:]
                J_geo_quat = kine_mat.dot(J_geo_rot)
                J = np.vstack((J_geo_pos, J_geo_quat))
            else:
                pose = pos
                J = J_geo_pos

            poses.append(pose)
            jacobs.append(J)
        return np.vstack(poses), np.vstack(jacobs)

[HiroIshida]

def util_set_robot_state(robot_model, joint_list, av, base_also=False):
    if base_also:
        av_joint, av_base = av[:-3], av[-3:] 
        x, y, theta = av_base
        robot_model.translation = np.array([x, y, 0.0])
        robot_model.rotation = rpy_matrix(theta, 0.0, 0.0)
    else:
        av_joint = av
    for joint, angle in zip(joint_list, av_joint):
        joint.joint_angle(angle)

def util_get_robot_state(robot_model, joint_list, base_also=False):
    av_joint = [j.joint_angle() for j in joint_list]
    if base_also:
        x, y, _ = robot_model.translation
        rpy = rpy_angle(robot_model.rotation)[0]
        theta = rpy[0]
        av_whole = np.hstack((av_joint, [x, y, theta]))
        return av_whole
    else:
        return av_joint

def inverse_kinematics_slsqp(self, 
                             target_coords,
                             link_list,
                             end_effector_cascaded_coords,
                             rot_also=True,
                             base_also=False
                             ):
    joint_list = [link.joint for link in link_list]
    joint_limits = [[j.min_angle, j.max_angle] for j in joint_list]
    world_coordinate = CascadedCoords()

    def endcoord_forward_kinematics(av, rot_also=True, base_also=False):
        if rot_also:
            raise Exception

        def quaternion_kinematic_matrix(q):
            # dq/dt = 0.5 * mat * omega 
            q1, q2, q3, q4 = q
            mat = np.array([
                [-q2, -q3, -q4], [q1, q4, -q3], [-q4, q1, q2], [q3, -q2, q1]])
            return mat * 0.5

        def compute_jacobian_wrt_world(av):
            util_set_robot_state(self, joint_list, av, base_also)
            base_link = self.link_list[0]
            J_joint = self.calc_jacobian_from_link_list([end_effector_cascaded_coords], link_list,
                                                  transform_coords=base_link,
                                                  rotation_axis=rot_also)
            if base_also:
                pos = end_effector_cascaded_coords.worldpos()
                J_base_pos = np.array([[1, 0, -pos[1]], [0, 1, pos[0]] [0, 0, 0]])
                if rot_also:
                    assert rot_also==False, "under construction"
                else:
                    return np.hstack((J_joint, J_base_pos))
            else:
                return J_joint

        def compute_pose_wrt_world(av):
            position = end_effector_cascaded_coords.worldpos()
            if rot_also:
                rotation = end_effector_cascaded_coords.worldcoords().quaternion
                return np.hstack((position, rotation))
            else:
                return position

        pose = compute_pose_wrt_world(av)
        jac = compute_jacobian_wrt_world(av)
        return pose, jac

    av_init = np.array([j.joint_angle() for j in joint_list])
    pos_target = target_coords.worldpos()
    quat_target = target_coords.worldcoords().quaternion if rot_also else None
    av_solved = inverse_kinematics_slsqp_common(
            util_get_robot_state(self, joint_list, base_also),
            endcoord_forward_kinematics, 
            joint_limits,
            pos_target, 
            quat_target)
    util_set_robot_state(self, joint_list, av_solved, base_also)
    return av_solved