Skip to content

Instantly share code, notes, and snippets.

@lanius

lanius/demo.py

Last active February 9, 2025 08:15
Show Gist options
  • Save lanius/cb8b5e0ede9ff3b2b2c1bc68b95066fb to your computer and use it in GitHub Desktop.
Save lanius/cb8b5e0ede9ff3b2b2c1bc68b95066fb to your computer and use it in GitHub Desktop.
Download ZIP
Sphere formed hexapod robot simulation using pybullet
Raw
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)
Raw
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>
Raw
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]
Raw
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])
Display the source blob
Display the rendered blob
Raw
shell.stl
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment