1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
|
import pybullet as p
import time
import math
# This simple snake logic is from some 15 year old Havok C++ demo
# Thanks to Michael Ewert!
p.connect(p.GUI)
plane = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, plane)
useMaximalCoordinates = True
sphereRadius = 0.25
#colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1
visualShapeId = -1
link_Masses = []
linkCollisionShapeIndices = []
linkVisualShapeIndices = []
linkPositions = []
linkOrientations = []
linkInertialFramePositions = []
linkInertialFrameOrientations = []
indices = []
jointTypes = []
axis = []
for i in range(36):
link_Masses.append(1)
linkCollisionShapeIndices.append(colBoxId)
linkVisualShapeIndices.append(-1)
linkPositions.append([0, sphereRadius * 2.0 + 0.01, 0])
linkOrientations.append([0, 0, 0, 1])
linkInertialFramePositions.append([0, 0, 0])
linkInertialFrameOrientations.append([0, 0, 0, 1])
indices.append(i)
jointTypes.append(p.JOINT_REVOLUTE)
axis.append([0, 0, 1])
basePosition = [0, 0, 1]
baseOrientation = [0, 0, 0, 1]
sphereUid = p.createMultiBody(mass,
colBoxId,
visualShapeId,
basePosition,
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis,
useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
anistropicFriction = [1, 0.01, 0.01]
p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
p.getNumJoints(sphereUid)
for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid, i)
p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)
dt = 1. / 240.
SNAKE_NORMAL_PERIOD = 0.1 #1.5
m_wavePeriod = SNAKE_NORMAL_PERIOD
m_waveLength = 4
m_wavePeriod = 1.5
m_waveAmplitude = 0.4
m_waveFront = 0.0
#our steering value
m_steering = 0.0
m_segmentLength = sphereRadius * 2.0
forward = 0
while (1):
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
m_steering = -.2
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
m_steering = 0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
m_steering = .2
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
m_steering = 0
amp = 0.2
offset = 0.6
numMuscles = p.getNumJoints(sphereUid)
scaleStart = 1.0
#start of the snake with smaller waves.
#I think starting the wave at the tail would work better ( while it still goes from head to tail )
if (m_waveFront < m_segmentLength * 4.0):
scaleStart = m_waveFront / (m_segmentLength * 4.0)
segment = numMuscles - 1
#we simply move a sin wave down the body of the snake.
#this snake may be going backwards, but who can tell ;)
for joint in range(p.getNumJoints(sphereUid)):
segment = joint #numMuscles-1-joint
#map segment to phase
phase = (m_waveFront - (segment + 1) * m_segmentLength) / m_waveLength
phase -= math.floor(phase)
phase *= math.pi * 2.0
#map phase to curvature
targetPos = math.sin(phase) * scaleStart * m_waveAmplitude
#// steer snake by squashing +ve or -ve side of sin curve
if (m_steering > 0 and targetPos < 0):
targetPos *= 1.0 / (1.0 + m_steering)
if (m_steering < 0 and targetPos > 0):
targetPos *= 1.0 / (1.0 - m_steering)
#set our motor
p.setJointMotorControl2(sphereUid,
joint,
p.POSITION_CONTROL,
targetPosition=targetPos + m_steering,
force=30)
#wave keeps track of where the wave is in time
m_waveFront += dt / m_wavePeriod * m_waveLength
p.stepSimulation()
time.sleep(dt)
|
