""" from AlexRobotics.planning import RandomTree as RPRT from AlexRobotics.dynamic import CustomManipulator as CM from AlexRobotics.control import RminComputedTorque as RminCTC import numpy as np import matplotlib.pyplot as plt ReComputeTraj = False saving_auto = False simulation = True name_traj = '../data/pendulum_traj.npy' #################################### R = CM.TestPendulum() R.ubar = np.array([0, 0, 0, 1]) x_start = np.array([-np.pi, 0, 0, 0, 0, 0]) x_goal = np.array([0, 0, 0, 0, 0, 0]) RRT = RPRT.RRT(R, x_start) T = 0.02 u_R1 = 0 u_R2 = 1 #RRT.U = np.array([ [ T,0,0,u_R1] , [ 0,0,0,u_R1] , [ -T,0,0,u_R1] ]) RRT.U = np.array([[T, 0, 0, u_R1], [0, 0, 0, u_R1], [-T, 0, 0, u_R1],
def __init__(self):
self.verbose = False
self.plot = True
# Message outgoing
self.pub_x = rospy.Publisher("x_hat", Float64MultiArray, queue_size=1)
# Messages Inputs
self.sub_a0 = rospy.Subscriber("a0/y",
dsdm_actuator_sensor_feedback,
self.feedback_callback_a0,
queue_size=1)
self.sub_a1 = rospy.Subscriber("a1/y",
dsdm_actuator_sensor_feedback,
self.feedback_callback_a1,
queue_size=1)
self.sub_a2 = rospy.Subscriber("a2/y",
dsdm_actuator_sensor_feedback,
self.feedback_callback_a2,
queue_size=1)
# Param Timer
self.timer = rospy.Timer(rospy.Duration.from_sec(2.0),
self.load_params)
# Load params
self.load_params(None)
# Load robot params
if self.robot_type == 'BoeingArm':
self.R = CM.BoeingArm()
self.plot_partial_config = True
elif self.robot_type == 'pendulum':
#self.R = CM.TestPendulum()
#self.plot_partial_config = False
self.R = CM.TestPendulum()
self.plot_partial_config = True
else:
print('Error loading robot type')
self.plot_partial_config = False
# Variable Init
self.a = np.zeros(self.R.dof)
self.da = np.zeros(self.R.dof)
self.q = np.zeros(self.R.dof)
self.dq = np.zeros(self.R.dof)
self.x_hat = self.R.q2x(self.q, self.dq)
if self.plot:
self.R.show_3D(self.q)
# Partial Manipulator
if self.plot_partial_config:
if self.robot_config == 'wrist-only':
self.Rp = Proto.SingleRevoluteDSDM()
self.qp = np.array([0])
elif self.robot_config == 'dual-plane':
self.Rp = Proto.TwoPlanarSerialDSDM()
self.qp = np.array([0, 0])
self.Rp.show(self.qp)
# -*- coding: utf-8 -*-
"""
Created on Sun Aug 14 13:27:30 2016
@author: agirard
"""
from AlexRobotics.dynamic import CustomManipulator as CM
from AlexRobotics.control import RminComputedTorque as RminCTC
import numpy as np
R = CM.BoeingArm()
R.mc0 = 1.0
x0 = np.array([2, 0, 0, 0, 0, 0])
# Assign controller
CTC_controller = RminCTC.RminComputedTorqueController(R)
R.ctl = CTC_controller.ctl
CTC_controller.w0 = 0.5
CTC_controller.zeta = 0.7
CTC_controller.n_gears = 2
R.plot3DAnimation(x0)
R.Sim.plot_CL('u')
def __init__(self):
self.verbose = True
# Sub
self.sub_set = rospy.Subscriber("setpoint" , joint_position , self.update_setpoint , queue_size=1 )
self.sub_enable = rospy.Subscriber("enable" , Bool , self.update_enable , queue_size=1 )
self.sub_mode = rospy.Subscriber("ctl_mode" , Int32 , self.update_mode , queue_size=1 )
self.sub_state_feedback = rospy.Subscriber("x_hat" , Float64MultiArray , self.update_state , queue_size=1 )
# Pub
#self.pub_control = rospy.Publisher("u", dsdm_robot_control_inputs , queue_size=1 )
self.pub_control = rospy.Publisher("a2/u", dsdm_actuator_control_inputs , queue_size=1 ) # Temp for testing, only one actuator
#self.pub_control = rospy.Publisher("CTC_U", dsdm_actuator_control_inputs , queue_size=1 )
self.pub_e = rospy.Publisher("ctc_error", ctl_error , queue_size=1 )
# Timer
#self.timer = rospy.Timer( rospy.Duration.from_sec(0.01), self.callback )
# Load ROS params
self.load_params( None )
# Assign controller
if self.robot_type == 'BoeingArm':
self.R = CM.BoeingArm()
self.CTC = RminCTC.RminComputedTorqueController( self.R )
elif self.robot_type == 'pendulum':
self.R = CM.TestPendulum()
self.CTC = RminCTC.RminComputedTorqueController( self.R )
# Temp to load traj
self.RRT = RPRT.RRT( self.R , np.array( [0,0,0,0,0,0] ) )
self.RRT.load_solution( '/home/alex/ROS_WS/src/dsdm_robotics/dsdm_control/data/pendulum_traj_test0.npy' )
else:
print('Error loading robot type')
# Load params
self.CTC.w0 = 8.0
self.CTC.zeta = 0.7
self.CTC.n_gears = 2 ##########
self.CTC.hysteresis = True
self.CTC.hys_level = 0.001
self.CTC.min_delay = 0.2
self.CTC.R.dq_max_HF = 1.3 # [rad/sec]
# Gear params
R_HS = self.CTC.R.R[0]
R_HF = self.CTC.R.R[1]
#self.CTC.last_gear_i = 0
#self.CTC.R.R = [ R_HS ] # only HF
self.R_HS = R_HS
self.R_HF = R_HF
# INIT
self.t_zero = rospy.get_rostime()
self.x = np.array([ 0 , 0 , 0 , 0 , 0 , 0])
self.ddq_d = np.array([ 0 , 0 , 0 ])
self.dq_d = np.array([ 0 , 0 , 0 ])
self.q_d = np.array([ 0 , 0 , 0 ])
self.enable = False
self.ctl_mode = 0
self.setpoint = 0
self.actual = 0
self.e = 0
self.de = 0
self.traj_started = False