Exemplo n.º 1
0
# -*- 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 robotcontrollers
###############################################################################

# Model
torque_controlled_robot = manipulator.ThreeLinkManipulator3D()

# Target
r_desired = np.array([0.5, 1, 2])

# Effector pid

model = torque_controlled_robot

effector_pid = robotcontrollers.EndEffectorPID(model)
effector_pid.rbar = r_desired
effector_pid.kp = np.array([100, 100, 100])
effector_pid.kd = np.array([30, 30, 30])
effector_pid.ki = np.array([90, 90, 90])

# Closed-loops
Exemplo n.º 2
0
# -*- 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.ThreeLinkManipulator3D()

ctl = nonlinear.ComputedTorqueController(sys)

# Target
ctl.rbar = np.array([0, 0, 0])

closed_loop_robot = ctl + sys

x0 = np.array([3.14, -3, 2, 0, 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, True)