def recover(n=N):
Lz = np.zeros(n + 1)
init_position = np.zeros((6, 3))
time.sleep(3)
# vrep.updateRobotPosition()
for i in range(6):
res, init_position[i] = vrep.simxGetObjectPosition(
clientID, S1[i], BCS, vrep.simx_opmode_oneshot_wait)
for i in range(1, n + 1):
Lz[i] = init_position[0][2] - i * 0.1 / n
for i in range(1, n + 1):
vrep.simxSynchronousTrigger(clientID)
for j in range(0, 6, 2):
vrep.simxSetObjectPosition(
clientID, Tip_target[j], BCS,
[init_position[j][0], init_position[j][1], Lz[i]],
vrep.simx_opmode_oneshot_wait)
for i in range(1, n + 1):
vrep.simxSynchronousTrigger(clientID)
for j in range(1, 6, 2):
vrep.simxSetObjectPosition(
clientID, Tip_target[j], BCS,
[init_position[j][0], init_position[j][1], Lz[i]],
vrep.simx_opmode_oneshot_wait)
def turn_a_deg(deg):
target = np.zeros((3, 3))
for i in range(0, 6, 2):
res, target[int(i / 2)] = vrep.simxGetObjectPosition(
clientID, S1[i], BCS, vrep.simx_opmode_oneshot_wait)
for i in range(3):
dist = math.sqrt(target[i][0]**2 + target[i][1]**2)
ang = math.atan2(target[i][1], target[i][0])
target[i][0] = dist * math.cos(ang + deg)
target[i][1] = dist * math.sin(ang + deg)
three_step(target, 0)
for i in range(1, 6, 2):
res, target[int(i / 2)] = vrep.simxGetObjectPosition(
clientID, S1[i], BCS, vrep.simx_opmode_oneshot_wait)
for i in range(3):
dist = math.sqrt(target[i][0]**2 + target[i][1]**2)
ang = math.atan2(target[i][1], target[i][0])
target[i][0] = dist * math.cos(ang + deg)
target[i][1] = dist * math.sin(ang + deg)
three_step(target, 1)
def print_steps():
import matplotlib.pyplot as plt
x = []
y = []
# vrep.updateRobotPosition()
for i in range(6):
res, Pos = vrep.simxGetObjectPosition(clientID, S1[i], BCS,
vrep.simx_opmode_oneshot_wait)
x.append(Pos[0])
y.append(Pos[1])
plt.plot(x, y, "d")
plt.show()
def transTo(target, n=N): #TODO: make max step length or
assert (target.shape == (6, 3))
initPos = np.zeros((6, 3))
# vrep.updateRobotPosition()
for i in range(6):
res, initPos[i] = vrep.simxGetObjectPosition(
clientID, S1[i], BCS, vrep.simx_opmode_oneshot_wait)
delta = (target - initPos) / n
# print (delta)
#To make the steps smoother
for i in range(3):
initPos += delta / 3
vrep.simxSynchronousTrigger(clientID)
for j in range(6):
vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS,
initPos[j],
vrep.simx_opmode_oneshot_wait)
for i in range(n - 2):
initPos += delta
vrep.simxSynchronousTrigger(clientID)
for j in range(6):
vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS,
initPos[j],
vrep.simx_opmode_oneshot_wait)
#To make the steps smoother
for i in range(3):
initPos += delta / 3
vrep.simxSynchronousTrigger(clientID)
for j in range(6):
vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS,
initPos[j],
vrep.simx_opmode_oneshot_wait)
for j in range(6):
vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS, target[j],
vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(clientID)
def three_step_delta(newpos, side, MOD="delta"):
"""
newpos is the difference between the new position and the present position of the three peds
3*3, the z
side = 0 or 1
assume that the body position is above the middle of the foot (x,y)s.
"""
initPos = np.zeros((6, 3))
# vrep.updateRobotPosition()
for i in range(6):
res, initPos[i] = vrep.simxGetObjectPosition(
clientID, S1[i], BCS, vrep.simx_opmode_oneshot_wait)
# height = 0.25
if (MOD == "delta"):
newpos_delta = newpos
else:
newpos_delta = np.zeros((3, 3))
for i in range(side, 6, 2):
newpos_delta[int(i / 2)] = newpos[int(i / 2)] - initPos[i]
newpos_delta = np.clip(newpos_delta, -0.1, 0.1)
avedelta = np.sum(newpos_delta, axis=0) / 6
target = initPos - avedelta
pee = []
for i in range(6):
if (i % 2 == side):
target[i] += newpos_delta[int(i / 2)]
pee += list(newpos_delta[int(i / 2)][:2])
# # transTo(target)
# print(pee)
# print(target)
peb = list(avedelta) + [0, 0, bodyDiffOri(target)
] #经验之举，否则转反了 #但是应该代表需要转的角度
peb[2] = 0
# pee = np.clip(pee,-0.1,0.1)
vrep.robotSetFoot(side, pee, peb)
time.sleep(2)
def target_orient():
_, loc = vrep.simxGetObjectPosition(clientID, goal, BCS,
vrep.simx_opmode_oneshot_wait)
print("loc:", loc)
return math.atan2(loc[1], loc[0])
def target_position():
return vrep.simxGetObjectPosition(clientID, goal, -1,
vrep.simx_opmode_oneshot_wait)[1][:2]
def robot_position():
return vrep.simxGetObjectPosition(clientID, BCS, -1,
vrep.simx_opmode_oneshot_wait)[1][:2]