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
|
from minitaur import Minitaur
import pybullet as p
import numpy as np
import time
import sys
import math
minitaur = None
evaluate_func_map = dict()
def current_position():
global minitaur
position = minitaur.getBasePosition()
return np.asarray(position)
def is_fallen():
global minitaur
orientation = minitaur.getBaseOrientation()
rotMat = p.getMatrixFromQuaternion(orientation)
localUp = rotMat[6:]
return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
def evaluate_desired_motorAngle_8Amplitude8Phase(i, params):
nMotors = 8
speed = 0.35
for jthMotor in range(nMotors):
joint_values[jthMotor] = math.sin(i * speed +
params[nMotors + jthMotor]) * params[jthMotor] * +1.57
return joint_values
def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
speed = 0.35
phaseDiff = params[2]
a0 = math.sin(i * speed) * params[0] + 1.57
a1 = math.sin(i * speed + phaseDiff) * params[1] + 1.57
a2 = math.sin(i * speed + params[3]) * params[0] + 1.57
a3 = math.sin(i * speed + params[3] + phaseDiff) * params[1] + 1.57
a4 = math.sin(i * speed + params[4] + phaseDiff) * params[1] + 1.57
a5 = math.sin(i * speed + params[4]) * params[0] + 1.57
a6 = math.sin(i * speed + params[5] + phaseDiff) * params[1] + 1.57
a7 = math.sin(i * speed + params[5]) * params[0] + 1.57
joint_values = [a0, a1, a2, a3, a4, a5, a6, a7]
return joint_values
def evaluate_desired_motorAngle_hop(i, params):
amplitude = params[0]
speed = params[1]
a1 = math.sin(i * speed) * amplitude + 1.57
a2 = math.sin(i * speed + 3.14) * amplitude + 1.57
joint_values = [a1, 1.57, a2, 1.57, 1.57, a1, 1.57, a2]
return joint_values
evaluate_func_map[
'evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase
evaluate_func_map[
'evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase
evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAngle_hop
def evaluate_params(evaluateFunc,
params,
objectiveParams,
urdfRoot='',
timeStep=0.01,
maxNumSteps=10000,
sleepTime=0):
print('start evaluation')
beforeTime = time.time()
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("%s/plane.urdf" % urdfRoot)
p.setGravity(0, 0, -10)
global minitaur
minitaur = Minitaur(urdfRoot)
start_position = current_position()
last_position = None # for tracing line
total_energy = 0
for i in range(maxNumSteps):
torques = minitaur.getMotorTorques()
velocities = minitaur.getMotorVelocities()
total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep
joint_values = evaluate_func_map[evaluateFunc](i, params)
minitaur.applyAction(joint_values)
p.stepSimulation()
if (is_fallen()):
break
if i % 100 == 0:
sys.stdout.write('.')
sys.stdout.flush()
time.sleep(sleepTime)
print(' ')
alpha = objectiveParams[0]
final_distance = np.linalg.norm(start_position - current_position())
finalReturn = final_distance - alpha * total_energy
elapsedTime = time.time() - beforeTime
print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
"finalReturn", finalReturn, "elapsed_time", elapsedTime)
return finalReturn
|
