In [1]:
from poppy.creatures import PoppyHumanoid
poppy = PoppyHumanoid(simulator='vrep')
%pylab inline
# the class time is used to set the time object to be the simulated time in V-REP and not the default python time
import time as real_time
class time:
def __init__(self,robot):
self.robot=robot
def time(self):
t_simu = self.robot.current_simulation_time
return t_simu
def sleep(self,t):
t0 = self.robot.current_simulation_time
while (self.robot.current_simulation_time - t0) < t-0.01:
real_time.sleep(0.001)
time = time(poppy)
print time.time()
time.sleep(0.025) #0.025 is the minimum step according to the V-REP defined dt
print time.time()
We need a primitive to run the force apply to poppy's chest.
In [2]:
from pypot.primitive import Primitive
class force_primitive(Primitive):
def __init__(self, robot, fx, fy, fz, shape):
self.robot = robot
self.fx = fx
self.fy = fy
self.fz = fz
self.shape = shape
Primitive.__init__(self, robot)
def run(self):
while not self.should_stop():
poppy.set_vrep_force([self.fx,self.fy,self.fz],self.shape)
time.sleep(0.025)
poppy.set_vrep_force([0,0,0],self.shape)
time.sleep(2)
To start and stop the force primitive :
In [10]:
force = force_primitive(poppy,-15,0,0,'chest_respondable')
In [11]:
force.start()
In [9]:
force.stop()
In [6]:
poppy.reset_simulation()
Prepared poppy for experiments
In [12]:
poppy.head_y.goto_position(0, 4, wait=False)
poppy.r_shoulder_y.goto_position(0, 4, wait=False)
poppy.l_shoulder_y.goto_position(0, 4, wait=False)
poppy.l_shoulder_x.goto_position(20, 4, wait=False)
poppy.r_shoulder_x.goto_position(-20, 4, wait=True)
Track the head like an IMU could do. On the next graph, you can see the pushing force every two seconds on the chest.
In [13]:
list_pos_x = []
list_pos_y = []
list_pos_z = []
t= []
pos = poppy.get_object_position('head_visual')
pos_x=pos[0]
pos_y=pos[1]
pos_z=pos[2]
t0 = time.time()
while time.time() - t0 < 20:
pos = poppy.get_object_position('head_visual')
if pos_x != pos[0]:
decalage_x=pos[0]-pos_x
decalage_y=pos[1]-pos_y
decalage_z=pos[2]-pos_z
list_pos_x.append(decalage_x)
list_pos_y.append(decalage_y)
list_pos_z.append(decalage_z)
pos_x = pos[0]
pos_y = pos[1]
pos_z = pos[2]
t.append(poppy.current_simulation_time)
time.sleep(0.01)
plot(t, list_pos_x)
plot(t, list_pos_y)
plot(t, list_pos_z)
legend(('dx', 'dy','dz'))
Out[13]:
Changing the reference frame from absolute to a frame relative to poppy (if you want to know more on matrix of rotation, you have this tutorial. Choosing the frame oriented by visual chest. The graph shows the same movement but in the relative frame and the arms try to decrease the ocillation by balancing the movement in the opposite direction.
In [ ]:
def rotate_matrix(a,b,g):
Rx = np.mat([[1,0,0], [0,cos(a),-sin(a)], [0,sin(a),cos(a)]])
Ry = np.mat([[cos(b),0,sin(b)], [0,1,0], [-sin(b),0,cos(b)]])
Rz = np.mat([[cos(g),-sin(g),0], [sin(g),cos(g),0], [0,0,1]])
Rot = Rz*Ry*Rx
return Rot
list_pos_x = []
list_pos_y = []
list_pos_z = []
t= []
pos0 = np.mat(poppy.get_object_position('head_visual')).transpose()
t0 = time.time()
time.sleep(0.01)
while time.time() - t0 < 20:
pos1 = np.mat(poppy.get_object_position('head_visual')).transpose()
if any(pos1 != pos0):
d_pos = pos1-pos0
#make a rotation to d_pos to transpose the movement in a relative frame (frame of chest_visual)
orient_chest = poppy.get_object_orientation('chest_visual')
Rot=rotate_matrix(-orient_chest[0],-orient_chest[1],-orient_chest[2])
d_pos=Rot*d_pos
#balance the movement with an opposite movement of the arm
#le terme instantanée doit être ajouté à un terme intégré sur les 10 derniers mouvements
poppy.r_shoulder_y.goal_position = d_pos[1]*2000
poppy.l_shoulder_y.goal_position = d_pos[1]*2000
poppy.r_shoulder_x.goal_position = -20-d_pos[2]*2000
poppy.l_shoulder_x.goal_position = 20-d_pos[2]*2000
list_pos_x.append(float(d_pos[0]))
list_pos_y.append(float(d_pos[1]))
list_pos_z.append(float(d_pos[2]))
t.append(poppy.current_simulation_time)
pos0 = pos1
time.sleep(0.01)
plot(t, list_pos_x)
plot(t, list_pos_y)
plot(t, list_pos_z)
legend(('dx', 'dy','dz'))
The balance use only the speed of the movement (i.e the movement of the arm is only proportional to the speed of the move of the head. It could be better to use also the acceleration and the position (integration of speed). So to be continued...
One test more with the possibility to react in function of the position and the acceleration.
In [ ]:
# choose the coefficient you want to apply to speed, position and acceleration. (P S A)
P = 200
S = 2000
A = 2000
class IMU:
def __init__(self,nb_record=10):
self.record_pos=[]
i=0
while i<nb_record:
self.record_pos.insert(0,0)
i+=1
def add_pos(self,d_pos):
size = len(self.record_pos)
self.record_pos.insert(0,d_pos)
del self.record_pos[size]
def speed(self):
return self.record_pos[0]
def acceleration(self):
return self.record_pos[0]-self.record_pos[1]
def position(self):
integrate = 0
for i in self.record_pos:
integrate += i
return integrate
def rotate_matrix(a,b,g):
Rx = np.mat([[1,0,0], [0,cos(a),-sin(a)], [0,sin(a),cos(a)]])
Ry = np.mat([[cos(b),0,sin(b)], [0,1,0], [-sin(b),0,cos(b)]])
Rz = np.mat([[cos(g),-sin(g),0], [sin(g),cos(g),0], [0,0,1]])
Rot = Rz*Ry*Rx
return Rot
list_pos_x = []
list_pos_y = []
list_pos_z = []
list_speed_x = []
list_speed_y = []
list_speed_z = []
list_acceleration_x = []
list_acceleration_y = []
list_acceleration_z = []
t= []
pos0 = np.mat(poppy.get_object_position('head_visual')).transpose()
t0 = time.time()
time.sleep(0.01)
IMU_x = IMU(20)
IMU_y = IMU(20)
IMU_z = IMU(20)
while time.time() - t0 < 15:
pos1 = np.mat(poppy.get_object_position('head_visual')).transpose()
if any(pos1 != pos0):
d_pos = pos1-pos0
#make a rotation to d_pos to transpose the movement in a relative frame (frame of chest_visual)
orient_chest = poppy.get_object_orientation('chest_visual')
Rot=rotate_matrix(-orient_chest[0],-orient_chest[1],-orient_chest[2])
d_pos=Rot*d_pos
# record the speed with the IMU class
IMU_x.add_pos(float(d_pos[0]))
IMU_y.add_pos(float(d_pos[1]))
IMU_z.add_pos(float(d_pos[2]))
#balance the movement with an opposite movement of the arm
poppy.r_shoulder_y.goal_position = IMU_y.speed()*S+IMU_y.position()*P+IMU_y.acceleration()*A
poppy.l_shoulder_y.goal_position = IMU_y.speed()*S+IMU_y.position()*P+IMU_y.acceleration()*A
poppy.r_shoulder_x.goal_position = -20-IMU_z.speed()*S-IMU_z.position()*P-IMU_z.acceleration()*A
poppy.l_shoulder_x.goal_position = 20-IMU_z.speed()*S-IMU_z.position()*P-IMU_z.acceleration()*A
# record for graph
list_pos_x.append(IMU_x.position())
list_pos_y.append(IMU_y.position())
list_pos_z.append(IMU_z.position())
list_speed_x.append(IMU_x.speed())
list_speed_y.append(IMU_y.speed())
list_speed_z.append(IMU_z.speed())
list_acceleration_x.append(IMU_x.acceleration())
list_acceleration_y.append(IMU_y.acceleration())
list_acceleration_z.append(IMU_z.acceleration())
t.append(poppy.current_simulation_time)
pos0 = pos1
time.sleep(0.01)
figure(1)
plot(t, list_pos_x)
plot(t, list_pos_y)
plot(t, list_pos_z)
legend(('dx', 'dy','dz'))
xlabel('time seconds')
title ('IMU position')
figure(2)
plot(t, list_speed_x)
plot(t, list_speed_y)
plot(t, list_speed_z)
legend(('dx', 'dy','dz'))
xlabel('time seconds')
title ('IMU speed')
figure(3)
plot(t, list_acceleration_x)
plot(t, list_acceleration_y)
plot(t, list_acceleration_z)
legend(('ddx', 'ddy','ddz'))
xlabel('time seconds')
title ('IMU acceleration')
What can I say, in conclusion : movement of the arm improve the stability but the movement of the arms have two different opposite effects : moving the gravity center and create an inertial force in the opposite direction of the moving of the gravity center. The inertial force is create during the movement but the move of the gravity center is more durable. The question is how to choose the better parameter for P, S, A and even nb_record to optimize the balance of poppy... no answer at the moment...
In [16]:
poppy.close()
In [ ]: