from poppy.creatures import Poppy4dofArmMini

"""
Explanation: How to reach a position with the maximum speed
Differences between goal_position / goto_position in minjerk / goto_position in dummy
End of explanation
"""


mini_dof = Poppy4dofArmMini()

for m in mini_dof.motors:
    m.compliant = False
    m.goto_position(0,1)

"""
Explanation: With the goto_position method, you can set an angle and a time. But you can not be absolutely sure that the position will be effectively reach. Because the priority is on the time. 
More precisely, if you are in minjerk mode for the goto_behavior the movement is a succession of goal position, so you can be confident about the position reach, but it is not certain. For example, if you overspeed the motor.
End of explanation
"""


for m in mini_dof.motors:
    m.pid=(4,1,0.1)

"""
Explanation: We need to adjust the PID of the motors, because if not you can have more than 3 degrees to difference between the present_position and the goal_position.
To know more, have a look on the activities : How to adjust the PID parameters for the motors.
End of explanation
"""


mini_dof.m3.goto_behavior= 'minjerk'

mini_dof.m3.goto_behavior

"""
Explanation: Minjerk behavior :
Check, the goto_behavior :
End of explanation
"""


from pypot.primitive import Primitive
import time
%pylab inline

class Graph_primitive(Primitive):
    def __init__(self,robot,motors_name):
        self.robot = robot
        Primitive.__init__(self, robot)
        self.fake_motors={}
        for name in motors_name:
            self.fake_motors[name] = getattr(self.robot, name)  
        self.position={}
        self.load={}
        self.speed={}
        
    def setup(self):
        for m in self.fake_motors.keys():
            self.position[m] = []
            self.speed[m] = []
            self.load[m] = []
        self.python_time=[]
        self.pypot_time=[]
    
    def run(self):
        t0 = time.time()
        while not self.should_stop():
            for m in self.fake_motors.keys():
                self.position[m].append(self.fake_motors[m].present_position)
                self.load[m].append(self.fake_motors[m].present_load)
                self.speed[m].append(self.fake_motors[m].present_speed)
            self.python_time.append(time.time()-t0)
            self.pypot_time.append(self.elapsed_time)
            time.sleep(0.02)
            
            

graph = Graph_primitive(mini_dof,['m3',])

"""
Explanation: Make a primitive to record data from motors :
End of explanation
"""


graph.start()
mini_dof.m3.goto_position(90,1,wait=True)
mini_dof.m3.goto_position(0,1)
graph.stop()

"""
Explanation: Do a simple movement :
End of explanation
"""


figure(1)
plot(graph.pypot_time,graph.position['m3'])
xlabel('elapsed time seconds')
ylabel('position')
title ('Record position function of elapsed time')

"""
Explanation: Display the graph :
End of explanation
"""


graph.start()
mini_dof.m3.goto_position(90,0.1,wait=True)
mini_dof.m3.goto_position(0,1,wait=True)
graph.stop()

figure(1)
plot(graph.pypot_time,graph.position['m3'])
xlabel('elapsed time seconds')
ylabel('position')
title ('Record position function of elapsed time')

"""
Explanation: But with :
End of explanation
"""


mini_dof.m3.goto_behavior = 'dummy'

"""
Explanation: The position is definitely not reach.
Dummy behavior
This is different in goto_behavior dummy. With this mode, there is not a succession of goal_position and time but only one command : A goal_position and a moving_speed. We can remark here that using the dummy mode certainly reduce the communication on the bus to the motors (could be a good thing).
Change the behavior
End of explanation
"""


graph.start()
mini_dof.m3.goto_position(90,1,wait=True)
mini_dof.m3.goto_position(0,1)
graph.stop()

"""
Explanation: Do a simple movement :
End of explanation
"""


figure(1)
plot(graph.pypot_time,graph.position['m3'])
xlabel('elapsed time seconds')
ylabel('position')
title ('Record position function of elapsed time')

"""
Explanation: Display the graph :
End of explanation
"""


graph.start()
for i in range(3):
    mini_dof.m3.goto_position(90,0.5,wait=True)
    mini_dof.m3.goto_position(-90,0.5,wait=True)
mini_dof.m3.goto_position(0,1,wait=True)
graph.stop()

figure(1)
plot(graph.pypot_time,graph.position['m3'])
xlabel('elapsed time seconds')
ylabel('position')
title ('Record position function of elapsed time')

"""
Explanation: You can see that the 90 degree is not reach in one second. So the movement
And with :
End of explanation
"""


def wait_position(motor) :
    while abs(motor.present_position-motor.goal_position) > 2: # the precision in degree is 2° , you can change but usually XL-320 motors have not a precision more than 1°
        time.sleep(0.02)

graph.start()
for i in range(3):
    mini_dof.m3.goto_position(90,0.5)
    wait_position(mini_dof.m3)
    mini_dof.m3.goto_position(-90,0.5,wait=True)
    wait_position(mini_dof.m3)
mini_dof.m3.goto_position(0,1,wait=True)
graph.stop()

figure(1)
plot(graph.pypot_time,graph.position['m3'])
xlabel('elapsed time seconds')
ylabel('position')
title ('Record position function of elapsed time')

"""
Explanation: This is awful ! The position are really far from desired position. Because, pypot calcule a theorical speed wich seems to be always under the real one.
If you want to put the priority to the position and not the time, you can add an instruction to wait until the position is reach :
End of explanation
"""


for m in mini_dof.motors:
    m.moving_speed = 100 # degrees per seconde

def wait_position(robot) :
    while abs(
        robot.m1.present_position-robot.m1.goal_position) > 5 or abs(
        robot.m2.present_position-robot.m2.goal_position) > 5 or abs(
        robot.m3.present_position-robot.m3.goal_position) >5 or abs(
        robot.m4.present_position-robot.m4.goal_position) >5 : # the precision in degree is 2° , you can change but usually XL-320 motors have not a precision more than 1°
        time.sleep(0.02)

for i in range(10):
    mini_dof.m1.goal_position = 90
    mini_dof.m2.goal_position = 10
    mini_dof.m3.goal_position = 80
    mini_dof.m4.goal_position = -40
    wait_position(mini_dof)
    for m in mini_dof.motors:
        m.goal_position = 0
    wait_position(mini_dof)
    mini_dof.m1.goal_position = -30
    mini_dof.m2.goal_position = 10
    mini_dof.m3.goal_position = 80
    mini_dof.m4.goal_position = -40
    wait_position(mini_dof)
    for m in mini_dof.motors:
        m.goal_position = 0
    wait_position(mini_dof)

for m in mini_dof.motors:
    m.moving_speed = 0 # degrees per seconde

mini_dof.close()

"""
Explanation: Positions are now respected (+/- : 2°) but time is not.
goal_position and moving_speed
Set the moving_speed of the motors :
End of explanation
"""


