Python ThreeLinkManipulator3D 예제들

예제 #1

파일: threelinkrobot_effector_pid_controller.py 프로젝트: SherbyRobotics/pyro

# -*- 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

예제 #2

파일: threelinkrobot_computed_torque_controller.py 프로젝트: echoix/pyro

# -*- 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)