# -*- coding: utf-8 -*-
"""
Created on Sun May 12 16:34:44 2019
@author: Alexandre
"""
###############################################################################
import numpy as np
###############################################################################
from pyro.control import robotcontrollers
from pyro.dynamic import manipulator
###############################################################################
torque_controlled_robot = manipulator.TwoLinkManipulator()
speed_controlled_robot = manipulator.SpeedControlledManipulator(
torque_controlled_robot)
kinematic_controller = robotcontrollers.EndEffectorKinematicController(
speed_controlled_robot, 1)
kinematic_controller.rbar = np.array([0.5, 0.5])
closed_loop_robot = kinematic_controller + speed_controlled_robot
x0 = np.array([-0.5, 0.2])
closed_loop_robot.plot_animation(x0, 5)
closed_loop_robot.sim.plot('xu')
#closed_loop_robot.rbar = np.array([1.5,0.5])
#closed_loop_robot.plot_animation( x0, 5 )
return dq_r
'''
#################################################################
################## Main ########
#################################################################
'''
if __name__ == "__main__":
""" MAIN TEST """
robot = manipulator.TwoLinkManipulator()
kp = 10
kd = 0
joint_pid = JointPID(2, kp , 0 , kd)
joint_pid.rbar = np.array([0,0])
kp = 200
kd = 0
effector_pid = EndEffectorPID( robot , kp , 0 , kd)
effector_pid.rbar = np.array([0.5,0.5])
#closed_loop_sys = joint_pid + robot
closed_loop_sys = effector_pid + robot
# -*- coding: utf-8 -*-
"""
Created on Sun May 12 16:34:44 2019
@author: Alexandre
"""
###############################################################################
import numpy as np
###############################################################################
from pyro.dynamic import manipulator
from pyro.control import nonlinear
###############################################################################
sys = manipulator.TwoLinkManipulator()
ctl = nonlinear.ComputedTorqueController(sys)
# Target
ctl.rbar = np.array([0, 0])
closed_loop_robot = ctl + sys
x0 = np.array([3.14, +1, 0, 0])
closed_loop_robot.plot_trajectory(x0, 5)
closed_loop_robot.sim.plot('x')
closed_loop_robot.sim.plot('u')
closed_loop_robot.animate_simulation(1)