Sphere formed hexapod robot simulation using pybullet

demo.py

import argparse
import os

import numpy as np
import pybullet as p

import motion


def rotate(p, deg):
    angle = np.deg2rad(deg)
    return np.dot(np.array([
        [np.cos(angle), -np.sin(angle), 0.],
        [np.sin(angle), np.cos(angle), 0.],
        [0., 0., 1.]
    ]), p)


class Shell(object):

    def close(self):
        begin = motion.pose(1.047, 0.015, 2.055)
        fold_j3 = motion.pose(1.047, 0.015, 2.443)
        fold_j2 = motion.pose(1.047, 0.698, 2.443)
        end = motion.pose(1.047, 2.000, 0.770)

        m = [begin]
        duration = 2
        m.extend(motion.lerp(begin, fold_j3, duration))
        m.extend(motion.lerp(fold_j3, fold_j2, duration))
        m.extend(motion.lerp(fold_j2, end, duration))
        m.extend([motion.pose(1.047, 2.159, 0.519)])
        return motion.OneTime(m)

    def ready_to_open(self):
        close = motion.pose(1.047, 2.159, 0.519)
        end = motion.pose(1.047, 1.8151, 1.1170)
        m = []
        m.extend(motion.lerp(close, end, 5))
        m.extend([end])
        return motion.OneTime(m)

    def open(self):
        ready = motion.pose(1.047, 1.8151, 1.1170)
        opened = motion.pose(1.047, 0.015, 2.055)
        m = []
        m.extend(motion.lerp(ready, opened, 10))
        m.extend([opened])
        return motion.OneTime(m)


class Leg(object):

    def __init__(self, lr_fmr, links, offsets=None):
        self.name = lr_fmr
        self.links = links
        self.offsets = [0, 0, 0] if offsets is None else offsets

        # body_to_femur, femur_to_tibia, tibia_to_tarsus
        self.angles = [0, 0, 0]

    @property
    def target_angles(self):
        return np.array(self.angles) + np.array(self.offsets)


class Robot(object):

    legs = []
    motion = None

    def __init__(self):
        self.legs = self.build_legs()

        links = [leg.links for leg in self.legs]
        l_f, l_m, l_r = [rotate([.0, .16, -.1], deg) for deg in [-60, 0, 60]]
        f_xy, m_xy, r_xy = [[l[0], l[1]] for l in [l_f, l_m, l_r]]
        z = -.1
        self.gait = motion.Gait(links, f_xy, m_xy, r_xy, z)
        self.shell = Shell()
        self.motion = self.gait.rest()

    def build_legs(self):

        def left_middle_links():
            p_0 = np.array([0, .08, -.04])
            p_1 = np.array([0, .12, -.04])
            p_2 = np.array([0, .08, -.10])
            p_3 = np.array([0, .02, -.13])

            l_0 = p_0
            l_1 = p_1 - p_0
            l_2 = p_2 - p_1
            l_3 = p_3 - p_2

            return [
                l_0,
                [0., np.linalg.norm(l_1), 0.],
                [0., np.linalg.norm(l_2), 0.],
                [0., np.linalg.norm(l_3), 0.]
            ]

        legs = []
        for lr_fmr, deg, reflect, offsets in [
                ('left_front', -60, 1, [1.047, 2.159, 0.519]),
                ('left_middle', 0, 1, [0., 2.159, 0.519]),
                ('left_rear', 60, 1, [-1.047, 2.159, 0.519]),
                ('right_front', -120, -1, [-1.047, -2.159, -0.519]),
                ('right_middle', 180, -1, [0., -2.159, -0.519]),
                ('right_rear', 120, -1, [1.047, -2.159, -0.519])]:
            l_0, l_1, l_2, l_3 = left_middle_links()
            links = [
                rotate(l_0, deg),
                np.array(l_1) * reflect,
                np.array(l_2) * reflect,
                np.array(l_3) * reflect
            ]
            legs.append(Leg(lr_fmr, links, offsets))
        return legs

    def walk(self):
        x = 1.
        y = 0.
        z = 0.
        vx = np.tanh(x) * .03  # clamp value
        vy = np.tanh(y) * .03
        vt = np.tanh(z) * np.deg2rad(5)
        self.motion = self.gait.tripod(vx, vy, vt)

    def rest(self):
        self.motion = self.gait.rest()

    def move_shell(self, mode):
        self.motion = {
            'close': self.shell.close,
            'ready_to_open': self.shell.ready_to_open,
            'open': self.shell.open,
        }.get(mode)()

    def proceed(self):
        self.motion.proceed()

    def publish(self, model):
        # body_to_femur, femur_to_tibia, tibia_to_tarsus
        indexes = {
            'left_front': [7, 8, 9],
            'left_middle': [11, 12, 13],
            'left_rear': [15, 16, 17],
            'right_front': [19, 20, 21],
            'right_middle': [23, 24, 25],
            'right_rear': [27, 28, 29],
        }
        for leg, angles in zip(self.legs, self.motion.pose()):
            leg.angles = angles
            p.setJointMotorControlArray(
                model,
                indexes[leg.name],
                p.POSITION_CONTROL,
                targetPositions=leg.target_angles,
                forces=[50, 50, 50])


def main(robot, bullet_dir_path):
    client = p.connect(p.GUI)
    p.setGravity(0, 0, -9.8)
    p.setRealTimeSimulation(1)
    p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)

    plane_urdf = os.path.join(bullet_dir_path, 'data', 'plane.urdf')
    angles = np.deg2rad(21)
    midpoint = 15
    height = 2
    upper_plane = p.loadURDF(
        plane_urdf,
        [-midpoint, 0, 0],
        p.getQuaternionFromEuler([0, 0, 0]))
    steep_plane = p.loadURDF(
        plane_urdf,
        [midpoint * np.cos(angles), 0, -midpoint * np.sin(angles)],
        p.getQuaternionFromEuler([0, angles, 0]))
    lower_plane = p.loadURDF(
        plane_urdf,
        [height * np.cos(angles) + midpoint, 0, -height * np.sin(angles)],
        p.getQuaternionFromEuler([0, 0, 0]))

    model = p.loadURDF(
        'description.urdf',
        [-0.3, 0, 0.2],
        p.getQuaternionFromEuler([0, 0, 0]),
        flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)

    distance = 1.0
    yaw = 45
    pitch = 30
    pos, orn = p.getBasePositionAndOrientation(model)
    p.resetDebugVisualizerCamera(distance, pitch, yaw, pos)

    pursuit = True
    handlers = {
        49: robot.walk,  # key '1'
        50: robot.rest,  # key '2'
        51: lambda: robot.move_shell('close'),  # key '3'
        52: lambda: robot.move_shell('ready_to_open'),  # key '4'
        53: lambda: robot.move_shell('open')  # key '5'
    }
    while True:
        try:
            robot.proceed()
            robot.publish(model)

            key = p.getKeyboardEvents()
            for k, s in key.items():
                if k in handlers and s == 3:  # pressed
                    handlers[k]()

            if pursuit:
                pos, orn = p.getBasePositionAndOrientation(model)
                p.resetDebugVisualizerCamera(distance, pitch, yaw, pos)

        except:
            p.disconnect()
            break


if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument('bullet_dir_path', help='bullet3 directory path')
    args = parser.parse_args()

    main(Robot(), args.bullet_dir_path)

description.urdf

<?xml version="1.0" ?>
<robot name="hexapod" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <link name="base"/>
  <joint name="base_to_body" type="fixed">
    <parent link="base"/>
    <child link="body"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="body">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.16 0.16 0.08"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.16 0.16 0.08"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="shell_top_left_front">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="body_to_shell_top_left_front" type="fixed">
    <parent link="body"/>
    <child link="shell_top_left_front"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="shell_top_left_middle">
    <visual>
      <origin rpy="0 0 1.047" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 0 1.047" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 1.047" xyz="0 0 0"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="body_to_shell_top_left_middle" type="fixed">
    <parent link="body"/>
    <child link="shell_top_left_middle"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="shell_top_left_rear">
    <visual>
      <origin rpy="0 0 2.094" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 0 2.094" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 2.094" xyz="0 0 0"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="body_to_shell_top_left_rear" type="fixed">
    <parent link="body"/>
    <child link="shell_top_left_rear"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="shell_top_right_rear">
    <visual>
      <origin rpy="0 0 3.142" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 0 3.142" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 3.142" xyz="0 0 0"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="body_to_shell_top_right_rear" type="fixed">
    <parent link="body"/>
    <child link="shell_top_right_rear"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="shell_top_right_middle">
    <visual>
      <origin rpy="0 0 4.189" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 0 4.189" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 4.189" xyz="0 0 0"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="body_to_shell_top_right_middle" type="fixed">
    <parent link="body"/>
    <child link="shell_top_right_middle"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="shell_top_right_front">
    <visual>
      <origin rpy="0 0 5.236" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 0 5.236" xyz="0 0 0"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 5.236" xyz="0 0 0"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="body_to_shell_top_right_front" type="fixed">
    <parent link="body"/>
    <child link="shell_top_right_front"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <joint name="body_to_left_front_femur" type="revolute">
    <parent link="body"/>
    <child link="left_front_femur"/>
    <origin rpy="0 0 -1.047" xyz="0.069 0.04 -0.04"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_front_femur">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_front_femur_to_tibia" type="revolute">
    <parent link="left_front_femur"/>
    <child link="left_front_tibia"/>
    <origin rpy="-2.159 0 0" xyz="0 0.04 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_front_tibia">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_front_tibia_to_tarsus" type="revolute">
    <parent link="left_front_tibia"/>
    <child link="left_front_tarsus"/>
    <origin rpy="-0.519 0 0" xyz="0 0.072 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_front_tarsus">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <mass value="3"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_front_tarsus_to_shell" type="fixed">
    <parent link="left_front_tarsus"/>
    <child link="shell_bottom_left_front"/>
    <origin rpy="2.678 0 0" xyz="0 0.067 0"/>
  </joint>
  <transmission name="trans_body_to_left_front_femur">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="body_to_left_front_femur">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_body_to_left_front_femur">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_left_front_femur_to_tibia">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_front_femur_to_tibia">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_left_front_femur_to_tibia">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_left_front_tibia_to_tarsus">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_front_tibia_to_tarsus">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_left_front_tibia_to_tarsus">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="body_to_left_middle_femur" type="revolute">
    <parent link="body"/>
    <child link="left_middle_femur"/>
    <origin rpy="0 0 0" xyz="0 0.08 -0.04"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_middle_femur">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_middle_femur_to_tibia" type="revolute">
    <parent link="left_middle_femur"/>
    <child link="left_middle_tibia"/>
    <origin rpy="-2.159 0 0" xyz="0 0.04 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_middle_tibia">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_middle_tibia_to_tarsus" type="revolute">
    <parent link="left_middle_tibia"/>
    <child link="left_middle_tarsus"/>
    <origin rpy="-0.519 0 0" xyz="0 0.072 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_middle_tarsus">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <mass value="3"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_middle_tarsus_to_shell" type="fixed">
    <parent link="left_middle_tarsus"/>
    <child link="shell_bottom_left_middle"/>
    <origin rpy="2.678 0 0" xyz="0 0.067 0"/>
  </joint>
  <transmission name="trans_body_to_left_middle_femur">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="body_to_left_middle_femur">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_body_to_left_middle_femur">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_left_middle_femur_to_tibia">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_middle_femur_to_tibia">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_left_middle_femur_to_tibia">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_left_middle_tibia_to_tarsus">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_middle_tibia_to_tarsus">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_left_middle_tibia_to_tarsus">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="body_to_left_rear_femur" type="revolute">
    <parent link="body"/>
    <child link="left_rear_femur"/>
    <origin rpy="0 0 1.047" xyz="-0.069 0.04 -0.04"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_rear_femur">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.02 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_rear_femur_to_tibia" type="revolute">
    <parent link="left_rear_femur"/>
    <child link="left_rear_tibia"/>
    <origin rpy="-2.159 0 0" xyz="0 0.04 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_rear_tibia">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.036 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_rear_tibia_to_tarsus" type="revolute">
    <parent link="left_rear_tibia"/>
    <child link="left_rear_tarsus"/>
    <origin rpy="-0.519 0 0" xyz="0 0.072 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="left_rear_tarsus">
    <visual>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.0335 0"/>
      <mass value="3"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="left_rear_tarsus_to_shell" type="fixed">
    <parent link="left_rear_tarsus"/>
    <child link="shell_bottom_left_rear"/>
    <origin rpy="2.678 0 0" xyz="0 0.067 0"/>
  </joint>
  <transmission name="trans_body_to_left_rear_femur">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="body_to_left_rear_femur">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_body_to_left_rear_femur">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_left_rear_femur_to_tibia">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_rear_femur_to_tibia">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_left_rear_femur_to_tibia">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_left_rear_tibia_to_tarsus">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_rear_tibia_to_tarsus">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_left_rear_tibia_to_tarsus">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="body_to_right_front_femur" type="revolute">
    <parent link="body"/>
    <child link="right_front_femur"/>
    <origin rpy="0 0 1.047" xyz="0.069 -0.04 -0.04"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_front_femur">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_front_femur_to_tibia" type="revolute">
    <parent link="right_front_femur"/>
    <child link="right_front_tibia"/>
    <origin rpy="2.159 0 0" xyz="0 -0.04 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_front_tibia">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_front_tibia_to_tarsus" type="revolute">
    <parent link="right_front_tibia"/>
    <child link="right_front_tarsus"/>
    <origin rpy="0.519 0 0" xyz="0 -0.072 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_front_tarsus">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <mass value="3"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_front_tarsus_to_shell" type="fixed">
    <parent link="right_front_tarsus"/>
    <child link="shell_bottom_right_front"/>
    <origin rpy="-2.678 0 0" xyz="0 -0.067 0"/>
  </joint>
  <transmission name="trans_body_to_right_front_femur">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="body_to_right_front_femur">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_body_to_right_front_femur">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_right_front_femur_to_tibia">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_front_femur_to_tibia">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_right_front_femur_to_tibia">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_right_front_tibia_to_tarsus">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_front_tibia_to_tarsus">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_right_front_tibia_to_tarsus">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="body_to_right_middle_femur" type="revolute">
    <parent link="body"/>
    <child link="right_middle_femur"/>
    <origin rpy="0 0 0" xyz="0 -0.08 -0.04"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_middle_femur">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_middle_femur_to_tibia" type="revolute">
    <parent link="right_middle_femur"/>
    <child link="right_middle_tibia"/>
    <origin rpy="2.159 0 0" xyz="0 -0.04 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_middle_tibia">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_middle_tibia_to_tarsus" type="revolute">
    <parent link="right_middle_tibia"/>
    <child link="right_middle_tarsus"/>
    <origin rpy="0.519 0 0" xyz="0 -0.072 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_middle_tarsus">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <mass value="3"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_middle_tarsus_to_shell" type="fixed">
    <parent link="right_middle_tarsus"/>
    <child link="shell_bottom_right_middle"/>
    <origin rpy="-2.678 0 0" xyz="0 -0.067 0"/>
  </joint>
  <transmission name="trans_body_to_right_middle_femur">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="body_to_right_middle_femur">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_body_to_right_middle_femur">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_right_middle_femur_to_tibia">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_middle_femur_to_tibia">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_right_middle_femur_to_tibia">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_right_middle_tibia_to_tarsus">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_middle_tibia_to_tarsus">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_right_middle_tibia_to_tarsus">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="body_to_right_rear_femur" type="revolute">
    <parent link="body"/>
    <child link="right_rear_femur"/>
    <origin rpy="0 0 -1.047" xyz="-0.069 -0.04 -0.04"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_rear_femur">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <geometry>
        <box size="0.04 0.04 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.02 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_rear_femur_to_tibia" type="revolute">
    <parent link="right_rear_femur"/>
    <child link="right_rear_tibia"/>
    <origin rpy="2.159 0 0" xyz="0 -0.04 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_rear_tibia">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <geometry>
        <box size="0.04 0.072 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.036 0"/>
      <mass value="0.5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_rear_tibia_to_tarsus" type="revolute">
    <parent link="right_rear_tibia"/>
    <child link="right_rear_tarsus"/>
    <origin rpy="0.519 0 0" xyz="0 -0.072 0"/>
    <axis xyz="1 0 0"/>
    <limit effort="2.8" lower="-2.6" upper="2.6" velocity="5.6"/>
  </joint>
  <link name="right_rear_tarsus">
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <geometry>
        <box size="0.04 0.067 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.0335 0"/>
      <mass value="3"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="right_rear_tarsus_to_shell" type="fixed">
    <parent link="right_rear_tarsus"/>
    <child link="shell_bottom_right_rear"/>
    <origin rpy="-2.678 0 0" xyz="0 -0.067 0"/>
  </joint>
  <transmission name="trans_body_to_right_rear_femur">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="body_to_right_rear_femur">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_body_to_right_rear_femur">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_right_rear_femur_to_tibia">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_rear_femur_to_tibia">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_right_rear_femur_to_tibia">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="trans_right_rear_tibia_to_tarsus">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_rear_tibia_to_tarsus">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_right_rear_tibia_to_tarsus">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="shell_bottom_left_front">
    <visual>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="shell_bottom_left_middle">
    <visual>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="shell_bottom_left_rear">
    <visual>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="3.142 0 2.094" xyz="0 -.02 .13"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="shell_bottom_right_front">
    <visual>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="shell_bottom_right_middle">
    <visual>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="shell_bottom_right_rear">
    <visual>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <geometry>
        <mesh filename="shell.stl" scale="1 1 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="3.142 0 5.236" xyz="0 .02 .13"/>
      <mass value="5"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <material name="black">
    <color rgba="0.0392156862745 0.0392156862745 0.0392156862745 1.0"/>
  </material>
  <material name="white">
    <color rgba="0.99 0.99 0.99 0.5"/>
  </material>
</robot>

kinema.py

from functools import reduce

import numpy as np


x = 0
y = 1
z = 2


def _calc_j_1_angle(l_0, l_1, l_2, l_3, target, link_is_left):
    A = - l_0[x] + target[x]
    B = - l_0[y] + target[y]
    C = - (l_1[x] + l_2[x] + l_3[x])
    d = .0001  # to avoid zero division
    sign = -1 if link_is_left else 1
    return 2 * np.arctan((B + sign * np.sqrt(A**2 + B**2 - C**2))/(A - C + d))


def _calc_j_3_angle(l_0, l_1, l_2, l_3, target, j_1, link_is_left):
    cos_j_1 = np.cos(j_1)
    sin_j_1 = np.sin(j_1)
    A = - 2.0 * (l_2[y] * l_3[y] + l_2[z] * l_3[z])
    B = + 2.0 * (l_2[y] * l_3[z] - l_2[z] * l_3[y])
    C = \
    + np.sum(np.power([l_0, l_1, target], 2)) \
    - np.sum(np.power([l_2, l_3], 2)) \
    - 2.0 * np.sum(np.multiply(l_0, target)) \
    + 2.0 * (l_0[z] * l_1[z] - l_2[x] * l_3[x] - l_1[z] * target[z]) \
    + 2.0 * (l_0[x] * l_1[x] + l_0[y] * l_1[y]) * cos_j_1 \
    - 2.0 * (l_1[x] * target[x] + l_1[y] * target[y]) * cos_j_1 \
    + 1.5 * (l_0[x] * l_1[y] - l_1[y] * target[x]) * sin_j_1 \
    + 2.0 * (- l_0[x] * l_1[y] + l_1[y] * target[x]) * sin_j_1**3 \
    + ( + ( + 3.5 * (- l_0[x] * l_1[y] + l_1[y] * target[x]) \
            + 2.0 * (l_0[y] * l_1[x] - l_1[x] * target[y]) \
        ) * sin_j_1 \
        + ( + 2.0 * (-l_0[y] * l_1[x] + l_1[x] * target[y]) \
            + 5.5 * (l_0[x] * l_1[y] - l_1[y] * target[x]) \
        ) * sin_j_1**3 \
        + 2.0 * (-l_0[x] * l_1[y] + l_1[y] * target[x]) * sin_j_1**5
    ) / cos_j_1**2
    sign = 1 if link_is_left else -1
    return 2*np.arctan((B + sign * np.sqrt(A**2 + B**2 - C**2))/(A - C))


def _calc_j_2_angle(l_0, l_1, l_2, l_3, target, j_3, link_is_left):
    cos_j_3 = np.cos(j_3)
    sin_j_3 = np.sin(j_3)
    A = - l_2[z] - l_3[y]*sin_j_3 - l_3[z]*cos_j_3
    B = - l_2[y] - l_3[y]*cos_j_3 + l_3[z]*sin_j_3
    C = - l_0[z] - l_1[z] + target[z]
    sign = 1 if link_is_left else -1
    return 2*np.arctan((B + sign * np.sqrt(A**2 + B**2 - C**2))/(A - C))


def calc_angles(l_0, l_1, l_2, l_3, target):
    link_is_left = l_0[y] > 0
    j_1 = _calc_j_1_angle(l_0, l_1, l_2, l_3, target, link_is_left)
    j_3 = _calc_j_3_angle(l_0, l_1, l_2, l_3, target, j_1, link_is_left)
    j_2 = _calc_j_2_angle(l_0, l_1, l_2, l_3, target, j_3, link_is_left)
    return [j_1, j_2, j_3]


def _trans(link):
    return np.array([
        [1., 0., 0., link[x]],
        [0., 1., 0., link[y]],
        [0., 0., 1., link[z]],
        [0., 0., 0., 1.]
    ])


def _x_rot(angle):
    return np.array([
        [1., 0., 0., 0.],
        [0., np.cos(angle), -np.sin(angle), 0.],
        [0., np.sin(angle), np.cos(angle), 0.],
        [0., 0., 0., 1.]
    ])


def _z_rot(angle):
    return np.array([
        [np.cos(angle), -np.sin(angle), 0., 0.],
        [np.sin(angle), np.cos(angle), 0., 0.],
        [0., 0., 1., 0.],
        [0., 0., 0., 1.]
    ])


def calc_position(l_0, l_1, l_2, l_3, j_1, j_2, j_3):
    matrices = [
        _trans(l_3), _x_rot(j_3),
        _trans(l_2), _x_rot(j_2),
        _trans(l_1), _z_rot(j_1),
        _trans(l_0)
    ]
    return reduce(
        lambda a, m: np.dot(m, a), matrices, np.array([0., 0., 0., 1.])
    )[:3]

motion.py

import numpy as np

import kinema


def a_to_p(links, angles):
    return [
        kinema.calc_position(l[0], l[1], l[2], l[3], a[0], a[1], a[2])
        for l, a in zip(links, angles)]


def p_to_a(links, positions):
    return [
        kinema.calc_angles(l[0], l[1], l[2], l[3], p)
        for l, p in zip(links, positions)]


def pose(j1, j2, j3):
    return [
        [-j1, -j2, -j3],
        [0., -j2, -j3],
        [j1, -j2, -j3],
        [j1, j2, j3],
        [0., j2, j3],
        [-j1, j2, j3]
    ]


def lerp(begin, end, n):
    return [[
        [np.interp(i, [0, n], [b, e]) for b, e in zip(bl, el)]
        for bl, el in zip(begin, end)] for i in range(n)]


def trans(p, dx, dy, dz, da):
    t = np.dot([
        [1., 0., 0., dx],
        [0., 1., 0., dy],
        [0., 0., 1., dz],
        [0., 0., 0., 1.]
    ], [  # z-axis rot
        [np.cos(da), -np.sin(da), 0., 0.],
        [np.sin(da), np.cos(da), 0., 0.],
        [0., 0., 1., 0.],
        [0., 0., 0., 1.]
    ])
    x, y, z, _ = np.dot(t, [p[0], p[1], p[2], 1])
    return (x, y, z)


class Runner(object):

    tick = 0
    index = 0

    def pose(self):
        ratio = self.tick / float(self.duration_of_a_pose)
        return [
            (np.array(c) * (1 - ratio)) + (np.array(n) * ratio)
            for c, n in zip(self.current_pose(), self.next_pose())]


class OneTime(Runner):

    def __init__(self, poses):

        self.poses = poses
        self.duration_of_a_pose = 300

    def proceed(self):
        self.tick += 1
        if self.tick > self.duration_of_a_pose:
            self.tick = 0
            if not self.index_is_last():
                self.index += 1

    def current_pose(self):
        return self.poses[self.index]

    def next_pose(self):
        if self.index_is_last():
            return self.poses[-1]
        return self.poses[self.index + 1]

    def index_is_last(self):
        return (self.index + 1) >= len(self.poses)


class Loop(Runner):

    def __init__(self, poses):
        self.poses = poses
        self.duration_of_a_pose = 500

    def proceed(self):
        self.tick += 1
        if self.tick > self.duration_of_a_pose:
            self.tick = 0
            self.index += 1

    def current_pose(self):
        return self.poses[self.index % len(self.poses)]

    def next_pose(self):
        return self.poses[(self.index + 1) % len(self.poses)]


class Gait(object):

    def __init__(self, links, f_xy, m_xy, r_xy, z):
        self.links = links
        self.f_x, self.f_y = f_xy
        self.m_x, self.m_y = m_xy
        self.r_x, self.r_y = r_xy
        self.z = z

    def base_positions(self):
        return np.array([
            [self.f_x, self.f_y, self.z],
            [self.m_x, self.m_y, self.z],
            [self.r_x, self.r_y, self.z],
            [self.f_x, -self.f_y, self.z],
            [self.m_x, -self.m_y, self.z],
            [self.r_x, -self.r_y, self.z]
        ])

    def rest(self):
        return OneTime([p_to_a(self.links, self.base_positions())])

    def tripod(self, vx, vy, vt):
        if np.linalg.norm([vx, vy, vt]) == 0:
            return self.rest()

        left_tripod = np.array([[  # 0, 2, 4
            [self.f_x, self.f_y, self.z],
            [self.r_x, self.r_y, self.z],
            [self.m_x, -self.m_y, self.z],
        ] for _ in range(6)])

        right_tripod = np.array([[  # 1, 3, 5
            [self.m_x, self.m_y, self.z],
            [self.f_x, -self.f_y, self.z],
            [self.r_x, -self.r_y, self.z]
        ] for _ in range(6)])

        def update_z(motion, indexes, dz):
            for i in indexes:
                motion[i] += [[0., 0., dz] for _ in range(3)]

        def update_x(motion, indexes, dx):
            for i in indexes:
                motion[i] += [[dx, 0., 0.] for _ in range(3)]

        def update_y(motion, indexes, dy):
            for i in indexes:
                motion[i] += [[0., dy, 0.] for _ in range(3)]

        def update_a(motion, indexes, da):
            rot = np.array([
                [np.cos(da), -np.sin(da), 0.],
                [np.sin(da), np.cos(da), 0.],
                [0., 0., 1.]
            ])
            for i in indexes:
                motion[i] = (np.dot(rot, motion[i].T)).T

        update_z(left_tripod, [0, 2], .02)
        update_z(left_tripod, [1], .06)
        update_x(left_tripod, [1, 2, 3], vx / 2)
        update_x(left_tripod, [4, 5, 0], -vx / 2)
        update_y(left_tripod, [1, 2, 3], vy / 2)
        update_y(left_tripod, [4, 5, 0], -vy / 2)
        update_a(left_tripod, [1, 2, 3], vt / 2)
        update_a(left_tripod, [4, 5, 0], -vt / 2)

        update_z(right_tripod, [3, 5], .03)
        update_z(right_tripod, [4], .06)
        update_x(right_tripod, [4, 5, 0], vx / 2)
        update_x(right_tripod, [1, 2, 3], -vx / 2)
        update_y(right_tripod, [4, 5, 0], vy / 2)
        update_y(right_tripod, [1, 2, 3], -vy / 2)
        update_a(right_tripod, [4, 5, 0], vt / 2)
        update_a(right_tripod, [1, 2, 3], -vt / 2)

        series_of_pos = [
            [lm[0], rm[0], lm[1], rm[1], lm[2], rm[2]]
            for lm, rm in zip(left_tripod, right_tripod)]

        return Loop([p_to_a(self.links, pos) for pos in series_of_pos])

shell.stl