def __init__(self):
rospy.init_node('CalibratorWithIMU', anonymous=True)
self.rate = 100 #[Hz]
self.ImuBoom = Imu()
self.ImuArm = Imu()
self.CalPosMsg = cmsg.JointValues()
self.CalJointMsg = JointState()
self.P_old = 5
self.P_new = []
self.posBoom = []
self.posArm = []
self.posBucket = []
self.velBoom = []
self.velArm = []
self.velBucket = []
self.alpha = 0.0
self.beta = 0.0
self.sub_IMUBoom = rospy.Subscriber('IMUBoom', Imu, self.cb_IMUBoom)
self.sub_IMUArm = rospy.Subscriber('IMUArm', Imu, self.cb_IMUArm)
self.sub_posEPOS = rospy.Subscriber('JointsEPOS', JointState,
self.cb_posEPOS)
self.sub_posDYNA = rospy.Subscriber('JointsDYNA', JointState,
self.cb_posDYNA)
self.pub_JointCalib2 = rospy.Publisher('CalibratedJointVals3',
JointState,
queue_size=10)
def __init__(self):
rospy.init_node('controller', anonymous=True)
self.rate = 100 #[Hz]
self.joy_val = cmsg.JointValues()
self.enabled = False
self.joy_switch_servo = []
self.joy_switch_impedance = []
self.arm_motor_position_ref = 0.0
self.boom_mode = 3
self.arm_mode = 3
self.time_switch_last = rospy.get_rostime()
self.sub_joy = rospy.Subscriber('joy_values', cmsg.JointValues,
self.cb_joy)
self.sub_joy_right = rospy.Subscriber('joy_right', smsg.Joy,
self.cb_joy_right)
self.sub_state_arduino = rospy.Subscriber('machine_state_arduino',
cmsg.JointStateMachineArduino, self.cb_state_arduino)
self.pub_arduino = rospy.Publisher('arduino_commands', cmsg.JointCommandArduino,
queue_size=10)
self.pub_dynamixel = rospy.Publisher('dynamixel_commands', cmsg.JointCommandDynamixel,
queue_size=10)
def __init__(self):
rospy.init_node('spd_com_teleop', anonymous=True)
self.rate = 100 #[Hz]
self.joy_val = cmsg.JointValues()
self.sub_joy = rospy.Subscriber('joy_values', cmsg.JointValues,
self.cb_joy)
self.pub_spd_com = rospy.Publisher('spd_commands',
cmsg.JointValues,
queue_size=10)
def __init__(self):
rospy.init_node('joy_msg_manager', anonymous=True)
self.test = rospy.get_param('~test', False)
self.joy_val_msg = cmsg.JointValues()
self.sub_joy_right = rospy.Subscriber('joy_right', smsg.Joy,
self.cb_joy_right)
self.sub_joy_left = rospy.Subscriber('joy_left', smsg.Joy,
self.cb_joy_left)
self.pub_joy_values = rospy.Publisher('joy_values',
cmsg.JointValues,
queue_size=10)
def update(self):
r = rospy.Rate(self.rate)
# Main Loop
while not rospy.is_shutdown():
spd_com = cmsg.JointValues()
spd_com.boom = self.joy_val.boom * 200.0
spd_com.arm = self.joy_val.arm * (-200.0)
spd_com.bucket = self.joy_val.bucket * 0.5
spd_com.swing = self.joy_val.swing * 0.0
self.pub_spd_com.publish(spd_com)
r.sleep()
def __init__(self):
rospy.init_node('controller', anonymous=True)
self.rate = 100 #[Hz]
self.joy_val = cmsg.JointValues()
self.enabled = False
self.joy_switch_servo = []
self.joy_switch_impedance = []
self.joy_switch_trajectory = []
self.arm_motor_position_ref = 0.0
self.boom_mode = 3
self.arm_mode = 3
self.buffer_length = 100
self.power_buffer = np.zeros(self.buffer_length)
self.regressor_buffer = np.zeros(self.buffer_length)
self.power_gradient = 0
self.power_gradient_last = 0
self.velocity_adaptation_last = 0
self.velocity_adaptation = 0
self.time_switch_last = rospy.get_rostime()
self.sub_joy = rospy.Subscriber('joy_values', cmsg.JointValues,
self.cb_joy)
self.sub_joy_right = rospy.Subscriber('joy_right', smsg.Joy,
self.cb_joy_right)
self.sub_state_arduino = rospy.Subscriber(
'machine_state_arduino', cmsg.JointStateMachineArduino,
self.cb_state_arduino)
self.pub_arduino = rospy.Publisher('arduino_commands',
cmsg.JointCommandArduino,
queue_size=10)
self.pub_dynamixel = rospy.Publisher('dynamixel_commands',
cmsg.JointCommandDynamixel,
queue_size=10)
self.pub_gradient = rospy.Publisher('power_gradient',
Float32,
queue_size=10)
self.pub_power = rospy.Publisher('power', Float32, queue_size=10)
def __init__(self):
rospy.init_node('controller', anonymous=True)
self.rate = 100 #[Hz]
self.joy_val = cmsg.JointValues()
self.joint_states = smsg.JointState()
self.joy_switch = []
self.bias_current_scaling = 700.0
self.internal_impedance = 500.0
self.Kmode = False
self.time_switch_last = rospy.get_rostime()
self.initial_position_boom = 0.0
self.sub_joy_right = rospy.Subscriber('joy_right', smsg.Joy,
self.cb_joy_right)
self.sub_joy = rospy.Subscriber('joy_values', cmsg.JointValues,
self.cb_joy)
self.sub_joints_EPOS = rospy.Subscriber('/joint_states_EPOS',
smsg.JointState, self.cb_state)
self.pub_com = rospy.Publisher('commands',
cmsg.JointCommand,
queue_size=10)
def __init__(self):
rospy.init_node('controller', anonymous=True)
self.rate = 100 #[Hz]
self.joy_val = cmsg.JointValues()
#initialise variables
self.enabled = False
self.joy_switch_servo = []
self.joy_switch_impedance = []
self.joy_switch_trajectory = []
self.boom_motor_position_ref = 0.0
self.arm_motor_position_ref = 0.0
self.boom_motor_velocity_ref = 0.0
self.arm_motor_velocity_ref = 0.0
self.ff = 0.97
self.robot_mode = 0
self.boom_mode = 3
self.arm_mode = 3
self.trajectory_stage = 0
self.boom_calibration = 0
self.arm_calibration = 0
self.bucket_calibration = 0
self.buffer_length = 100
self.power_buffer = np.zeros(self.buffer_length)
self.regressor_buffer = np.zeros(self.buffer_length)
self.power_gradient = 0
self.power_gradient_last = 0;
self.velocity_adaptation_last = 0;
self.velocity_adaptation= 0;
self.RLS_cov_last = np.matrix(np.identity(2))
self.RLS_estimate_last = np.transpose(np.matrix([0,0]))
print(self.RLS_estimate_last)
self.time_switch_last = rospy.get_rostime()
self.time_switch_last_trajectory = rospy.get_rostime()
self.sub_joy = rospy.Subscriber('joy_values', cmsg.JointValues,
self.cb_joy)
self.sub_joy_right = rospy.Subscriber('joy_right', smsg.Joy,
self.cb_joy_right)
self.sub_state_arduino = rospy.Subscriber('machine_state_arduino',
cmsg.JointStateMachineArduino, self.cb_state_arduino)
self.sub_pos_dynamixel = rospy.Subscriber('joint_states_dynamixel', JointState, self.cb_pos_dynamixel);
self.sub_joint_calibration = rospy.Subscriber('JointCalibration',
cmsg.JointCalibration, self.cb_joint_calibration)
self.pub_arduino = rospy.Publisher('arduino_commands', cmsg.JointCommandArduino,
queue_size=10)
self.pub_dynamixel = rospy.Publisher('dynamixel_commands', cmsg.JointCommandDynamixel,
queue_size=10)
self.pub_BATCH = rospy.Publisher('power_gradient_batch', Float32, queue_size=10)
self.pub_RLS = rospy.Publisher('power_gradient_rls', Float32, queue_size=10)
self.pub_power = rospy.Publisher('power', Float32, queue_size=10)