def __init__(self):
rospy.init_node('joint_states_publisher')
motors_list = rospy.get_param('~motors_list')
joint_state_publisher_topic = rospy.get_param('~joint_state_topic', '/dynamixel/joint_states')
urdf_file_path = rospy.get_param('~urdf_file_path', '')
self.joint_state = {}
if len(urdf_file_path) == 0:
robot = URDF.from_parameter_server()
else:
robot = URDF.from_xml_string(open(urdf_file_path,'r').read())
for joint in robot.joints:
joint_state = JointState()
joint_state.name = joint.name
joint_state.current_pos = 0
self.joint_state[joint.name] = joint_state
for motor in motors_list:
joint_state_topic = '/dynamixel/' + motor + '/state'
rospy.Subscriber(joint_state_topic, JointState, self.state_callback)
self.publisher = rospy.Publisher(joint_state_publisher_topic, SensorJointState, queue_size=1)
self.fix_publisher = rospy.Publisher('/joint_states', SensorJointState, queue_size=1)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
self.publish_joint_states()
rate.sleep()
def steering_state_cb(data):
js = JointState()
js.name = data.name
js.motor_ids = data.motor_ids
js.current_pos = data.current_pos
self.steer_state[js.motor_ids[0]] = js
def __init__(self):
####################
#外部制御変数定義
#座標変数
self.x=0.00
self.y=0.10
self.z=0.20
M_PI = math.pi #3.14
self.wrist_arg = M_PI/2
#動作モードオプション
self.Step=1 #移動量分割数
self.WaitMove=True #移動後の安定待ちON/OFF
#エラーフラグ
self.Error_Coordinate=False
####################
#内部制御変数定義
#SERVO制御変数(TILT)
self.Tilt=np.zeros(6,dtype=float)
#移動中監視
self.Tilt_Moving=np.zeros(6,dtype=int)
####################
#ROS設定
rospy.init_node('move_arm', anonymous=True)
self.RosRate = rospy.Rate(100)
self.Tilt_Pub=[0,0,0,0,0,0]
self.Tilt_Pub[1] = rospy.Publisher('tilt1_controller/command', Float64, queue_size=1000)
self.Tilt_Pub[2] = rospy.Publisher('tilt2_controller/command', Float64, queue_size=1000)
self.Tilt_Pub[3] = rospy.Publisher('tilt3_controller/command', Float64, queue_size=1000)
self.Tilt_Pub[4] = rospy.Publisher('tilt4_controller/command', Float64, queue_size=1000)
self.Tilt_Pub[5] = rospy.Publisher('tilt5_controller/command', Float64, queue_size=1000)
rospy.Subscriber('tilt1_controller/state',JointState, self.Tilt1_Callback)
rospy.Subscriber('tilt2_controller/state',JointState, self.Tilt2_Callback)
rospy.Subscriber('tilt3_controller/state',JointState, self.Tilt3_Callback)
rospy.Subscriber('tilt4_controller/state',JointState, self.Tilt4_Callback)
rospy.Subscriber('tilt5_controller/state',JointState, self.Tilt5_Callback)
self.Tilt_State=[0,0,0,0,0,0]
self.Tilt_State[1]=JointState()
self.Tilt_State[2]=JointState()
self.Tilt_State[3]=JointState()
self.Tilt_State[4]=JointState()
self.Tilt_State[5]=JointState()
#初回Topic購読を待ってInit完了(安定待ち)
self.wait_move()
for ch in range(1,6):
self.Tilt[ch]=self.Tilt_State[ch].current_pos
def _BroadcastJointStateinfo_P4(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = 0-((float(lineParts[1])* 0.00174532925)-1.57)
P2 = 0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = float(lineParts[3])
P4 = float(lineParts[4])
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P4_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
#rospy.logwarn(val)
Joint_State = JointState()
Joint_State.name = "right_arm_rotate_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P4_JointPublisher.publish(Joint_State)
except:
rospy.logwarn("Unexpected error:4" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P1(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = 0-((float(lineParts[1])* 0.00174532925)-(2.68 + self.cal_pan))# position
P2 = 0-((float(lineParts[2])* 0.00174532925)- (2.68 + self.cal_pan))# target
P3 = float(lineParts[3])# current
P4 = float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
Joint_State = JointState()
Joint_State.name = "right_arm_pan_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P1_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:pan" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P1(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = ((321-float(lineParts[1]))*0.008)-0.15
P2 = self.left_tilt
P3 = float(lineParts[3])# current
P4 = 0#float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P1_MotorPublisher.publish(Motor_State)
self._left_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P1_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_tilt_joint" + str(sys.exc_info()[0]))
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace,
port_namespace)
self.motor_id = rospy.get_param(self.controller_namespace +
'/motor/id')
self.initial_position_raw = rospy.get_param(self.controller_namespace +
'/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/max')
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace +
'/motor/acceleration')
else:
self.acceleration = None
if rospy.has_param(self.controller_namespace + '/motor/pid'):
self.pid_gains = rospy.get_param(self.controller_namespace +
'/motor/pid')
else:
self.pid_gains = []
if rospy.has_param('cute_servo_version'):
self.servo_version = rospy.get_param('cute_servo_version')
else:
self.servo_version = 'xqtor_1'
self.flipped = self.min_angle_raw > self.max_angle_raw
self.joint_state = JointState(name=self.joint_name,
motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace,
port_namespace)
self.motor_id = rospy.get_param(self.controller_namespace +
'/motor/id')
self.initial_position_raw = rospy.get_param(self.controller_namespace +
'/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/max')
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace +
'/motor/acceleration')
else:
self.acceleration = None
# added motor bias [AHC: 20160825]
if rospy.has_param(self.controller_namespace + '/motor/bias'):
self.motor_bias = rospy.get_param(self.controller_namespace +
'/motor/bias')
else:
self.motor_bias = 0.0
rospy.loginfo(
"JointPositionController::JointPositionController() - Motor %d bias set to %.4f\n",
self.motor_id, self.motor_bias)
self.flipped = self.min_angle_raw > self.max_angle_raw
self.joint_state = JointState(name=self.joint_name,
motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace,
port_namespace)
self.master_id = rospy.get_param(self.controller_namespace +
'/motor_master/id')
self.master_initial_position_raw = rospy.get_param(
self.controller_namespace + '/motor_master/init')
self.master_min_angle_raw = rospy.get_param(self.controller_namespace +
'/motor_master/min')
self.master_max_angle_raw = rospy.get_param(self.controller_namespace +
'/motor_master/max')
self.slave_id = rospy.get_param(self.controller_namespace +
'/motor_slave/id')
self.slave_offset = rospy.get_param(
self.controller_namespace + '/motor_slave/calibration_offset', 0)
if (self.slave_id == 9
): #Using unchanged normal operations for gripper motors
self.normal_slave = True
else:
self.normal_slave = False
self.flipped = self.master_min_angle_raw > self.master_max_angle_raw
self.joint_state = JointState(
name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
def __init__(self, name='hand1'):
prefix = 'reflex_' + name
self.prefix = prefix
rospy.init_node(prefix)
rospy.loginfo('Starting up the ReFlex SF hand')
self.motors = dict((prefix + suffix, Motor('/' + prefix + suffix))
for suffix in ['_f1', '_f2', '_f3', '_preshape'])
#attach callbacks to motor data
for motor in self.motors:
self.motors[
motor].subscriber_hook = lambda name, data: self.motorReceiveCb(
name, data)
rospy.Subscriber('/' + prefix + '/command', Command, self.receiveCmdCb)
self.state = JointState()
self.state.header.seq = 0
self.state.header.stamp = rospy.Time.now()
self.state.name = '/' + prefix
self.state.motor_ids = [-1] * 4
self.state.motor_temps = [-1] * 4
self.state.goal_pos = [0] * 4
self.state.current_pos = [0] * 4
self.state.error = [0] * 4
self.state.velocity = [0] * 4
self.state.load = [0] * 4
self.state.is_moving = False
self.pub = rospy.Publisher('/' + prefix + '/state',
JointState,
queue_size=10)
rospy.loginfo('ReFlex SF hand has started, waiting for commands...')
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace,
port_namespace)
self.msg = 0 # in order to keep the torque up and running we need to save this value.
self.motor_id = rospy.get_param(self.controller_namespace +
'/motor/id')
self.initial_position_raw = rospy.get_param(self.controller_namespace +
'/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/max')
self.min_angle_treshold = rospy.get_param(self.controller_namespace +
'/motor/minAngle')
self.max_angle_treshold = rospy.get_param(self.controller_namespace +
'/motor/maxAngle')
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace +
'/motor/acceleration')
else:
self.acceleration = None
self.flipped = self.min_angle_raw > self.max_angle_raw
self.joint_state = JointState(name=self.joint_name,
motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace,
port_namespace)
self.motor_id = rospy.get_param(self.controller_namespace +
'/motor/id')
self.initial_position_raw = rospy.get_param(self.controller_namespace +
'/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/max')
self.min_angle_treshold = rospy.get_param(self.controller_namespace +
'/motor/minAngle')
self.max_angle_treshold = rospy.get_param(self.controller_namespace +
'/motor/maxAngle')
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace +
'/motor/acceleration')
else:
self.acceleration = None
self.flipped = self.min_angle_raw > self.max_angle_raw
self.joint_state = JointState(name=self.joint_name,
motor_ids=[self.motor_id])
def __init__(self, port_handler, packet_handler, controller_namespace,
port_namespace):
DynamixelJointController.__init__(self, port_handler, packet_handler,
controller_namespace, port_namespace)
print("HELLO from JOINT_POSITION_CONTROLLER")
rospy.loginfo("MOTOR ID: " + self.controller_namespace + '/motor/id')
self.motor_id = rospy.get_param(self.controller_namespace +
'/motor/id')
self.model_number = rospy.get_param('/dynamixel/' +
self.port_namespace +
'/motor_info/' +
str(self.motor_id) +
'/model_number')
rospy.logwarn("MODEL_NUM::: " + str(self.model_number))
self.model_name = MODEL_NUMBER_2_MOTOR_NAME[self.model_number]
self.initial_position_raw = rospy.get_param(self.controller_namespace +
'/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/max')
self.torque_limit = 1.0 # TODO: Irvin Fix
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace +
'/motor/acceleration')
else:
self.acceleration = None
self.flipped = self.min_angle_raw > self.max_angle_raw
self.joint_state = JointState(name=self.joint_name,
motor_ids=[self.motor_id]) # IRVIN
def _Broadcast_left_j2_joint(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(P1)
if (partsCount < 2):
pass
try:
P1 = 0 - (radians(float(lineParts[1])) - 1.57)
Joint_State = JointStateDY()
Joint_State.name = "left_j2_joint"
Joint_State.current_pos = P1
Joint_State.header.stamp = rospy.Time.now()
self._P2_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:left_j2_joint" +
str(sys.exc_info()[0]))
def draw(self, display, new_base=(0, 0)):
'''Draw the whole arm'''
# Update positions given current
pygame.draw.line(display, (255, 162, 0), round_point(self.base),
round_point(self.base_endpoint), 3)
pygame.draw.line(display, (255, 255, 255), round_point(self.base),
round_point(self.starting), 1)
self.base_state_pub.publish(JointState(current_pos=self.base_angle))
def _Broadcast_Right_gripper_Joint(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 2):
pass
try:
P1 = radians(float(lineParts[1])) + 1.57
Joint_State = JointStateDY()
Joint_State.name = "right_gripper_joint"
Joint_State.current_pos = P1
Joint_State.header.stamp = rospy.Time.now()
self._P8_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:right_gripper_joint" +
str(sys.exc_info()[0]))
def _timed_out(self, te):
rospy.loginfo('Gripper movement timed out')
self._timer = None
if not self._last_state:
self._last_state = JointState()
result = GripperCommandResult()
self._as.set_aborted(result=self._state_to_feedback(
self._last_state, result),
text='Timed out')
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id')
self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max')
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration')
else:
self.acceleration = None
# added motor compliance slope and compliance margin [RX:02142019]
if rospy.has_param(self.controller_namespace + '/joint_compliance_slope'):
self.compliance_slope = rospy.get_param(self.controller_namespace + '/joint_compliance_slope', None)
if rospy.has_param(self.controller_namespace + '/joint_compliance_margin'):
self.compliance_margin = rospy.get_param(self.controller_namespace + '/joint_compliance_margin', None)
# added motor pid parameters [RX:10082018]
self.p_gain = None
self.i_gain = None
self.d_gain = None
if rospy.has_param(self.controller_namespace + '/motor/p_gain'):
self.p_gain = rospy.get_param(self.controller_namespace + '/motor/p_gain', None)
if rospy.has_param(self.controller_namespace + '/motor/i_gain'):
self.i_gain = rospy.get_param(self.controller_namespace + '/motor/i_gain', None)
if rospy.has_param(self.controller_namespace + '/motor/d_gain'):
self.d_gain = rospy.get_param(self.controller_namespace + '/motor/d_gain', None)
# added motor bias [AHC: 20160825]
if rospy.has_param(self.controller_namespace + '/motor/bias'):
self.motor_bias = rospy.get_param(self.controller_namespace + '/motor/bias')
else:
self.motor_bias = 0.0
# added motor torque limit
if rospy.has_param(self.controller_namespace + '/joint_torque_limit'):
self.torque_limit = rospy.get_param(self.controller_namespace + '/joint_torque_limit')
else:
self.torque_limit = 1.0
# added joint torque enabled [RX:02142019]
if rospy.has_param(self.controller_namespace + '/joint_torque_enabled'):
self.torque_enabled = rospy.get_param(self.controller_namespace + '/joint_torque_enabled')
else:
self.torque_enabled = True
rospy.loginfo("JointPositionController::JointPositionController() - Motor %d bias set to %.4f\n", self.motor_id, self.motor_bias)
if self.compliance_slope is not None:
rospy.loginfo("JointPositionController::JointPositionController() - Motor %d Compliance slope set to %d\n", self.motor_id, self.compliance_slope)
if self.compliance_margin is not None:
rospy.loginfo("JointPositionController::JointPositionController() - Motor %d Compliance margin set to %d\n", self.motor_id, self.compliance_margin)
self.flipped = self.min_angle_raw > self.max_angle_raw
self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def _HandleJoint_1_Command(self, data):
""" republish position. """
neck =(data)
self.gear_ratio = 0.3
new_pos =float (data.current_pos) * self.gear_ratio
new_goal=float (data.goal_pos) * self.gear_ratio
new_error = new_goal - new_pos
jointstate = JointState()
jointstate.name =("head_tilt_mod_joint")
jointstate.motor_ids =(neck.motor_ids)
jointstate.motor_temps =(neck.motor_temps)
jointstate.goal_pos =(new_goal)
jointstate.current_pos =(new_pos)
jointstate.velocity =(neck.velocity)
jointstate.load =(neck.load)
jointstate.is_moving =(neck.is_moving)
jointstate.error =(new_error)
#rospy.loginfo(jointstate)
self._neckPublisher.publish(jointstate)
def _handle_preempt(self):
if self._timer:
self._timer.shutdown()
self._timer = None
rospy.loginfo('Gripper movement preempted')
if not self._last_state:
self._last_state = JointState()
result = GripperCommandResult()
self._as.set_preempted(result=self._state_to_feedback(
self._last_state, result),
text='Preempted')
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id')
self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max')
self.flipped = self.min_angle_raw > self.max_angle_raw
self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.motor_id = self.declare_parameter('motor.id').value
self.initial_position_raw = self.declare_parameter('motor.init').value
self.min_angle_raw = self.declare_parameter('motor.min').value
self.max_angle_raw = self.declare_parameter('motor.max').value
self.flipped = self.min_angle_raw > self.max_angle_raw
self.last_commanded_torque = 0.0
self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, number):
self.number = number
#if(number==1):
self.bridge = CvBridge()
self.bridge2 = CvBridge()
self.flipperVelFront = Float64()
self.flipperVelRear = Float64()
self.startStopRobot = Bool()
self.startStopRobot.data = False
self.VERBOSE = True
self.robotGroundMap = np.zeros((28, 28), dtype="uint16")
self.currentPose = Odometry()
self.imu_data = Imu()
self.accelZ = 0
self.acceleration_to_high = 0
self.tip_over_angle = math.pi / 4 + math.pi / 6
self.robot_flip_over = False
self.biggestangularAccelz = 0
self.goalPose = Odometry()
self.flipperPoseFront = JointState()
self.flipperPoseRear = JointState()
self.countPub = 0
self.robotAction = 7
self.main()
self.last_angular_velocity_y = 0
self.last_nsecs = 0
self.last_time = time.time()
self.mean_angl_vel_y = Memory(5)
self.mean_angl_vel_y.resetBuffer()
self.mean_flipper_front_angl_vel_y = Memory(5)
self.mean_flipper_front_angl_vel_y.resetBuffer()
self.mean_flipper_rear_angl_vel_y = Memory(5)
self.mean_flipper_rear_angl_vel_y.resetBuffer()
self.startStopRobotPub.publish(True)
def __init__(self, dxl_io, controller_namespace, port_namespace):
self.running = False
self.dxl_io = dxl_io
self.controller_namespace = controller_namespace
self.port_namespace = port_namespace
self.motor_id = rospy.get_param(self.controller_namespace +
'/motor/id')
self.joint_name = rospy.get_param(self.controller_namespace +
'/joint_name')
self.lock = threading.Lock()
# joint_state
self.joint_state = JointState(name=self.joint_name,
motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id')
self.master_initial_position_raw = rospy.get_param(self.controller_namespace + '/motor_master/init')
self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min')
self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max')
self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id')
self.flipped = self.master_min_angle_raw > self.master_max_angle_raw
self.last_commanded_torque = 0.0
self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
def msg_callback_left_finger(self, msg):
out_msg = JointState()
out_msg.header = msg.header
out_msg.goal_pos = msg.set_point
out_msg.current_pos = msg.process_value
out_msg.error = msg.error
#we still need to obtain the speed information about the joint...
#motor IDs are filled according to a joint of the arm
out_msg.motor_ids.append(8)
#other stuff that is not known
out_msg.load=0
out_msg.is_moving=0 # we should obtain this information somehow
out_msg.motor_temps.append(0)
self.pub_left_finger.publish(out_msg)
def _BroadcastJointStateinfo_P5(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = 0 - ((radians((float(lineParts[1]))) / 10) - 0.65)
P2 = self.right_rotate #0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = float(lineParts[3])
P4 = 0
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P5_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
self._right_rotate_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "right_arm_rotate_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P5_JointPublisher.publish(Joint_State)
#rospy.logwarn(val)
except:
rospy.logwarn("Unexpected error:right_arm_rotate_joint" +
str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P2(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 4):
pass
try:
P1 = radians(float(lineParts[1]) * 0.2)
P2 = self.right_tilt
P3 = 0 #float(lineParts[3])# current
P4 = 0 #float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
self._right_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "right_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now(
) #.stamprospy.Time.from_sec(state.timestamp)
self._P2_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:right_arm_tilt_joint" +
str(sys.exc_info()[0]))
def __init__(self, dxl_io, controller_namespace, port_namespace):
self.running = False
self.dxl_io = dxl_io
self.controller_namespace = controller_namespace
self.port_namespace = port_namespace
# Get params from yaml file
self.joint_name = rospy.get_param(self.controller_namespace +
'/joint_name')
self.joint_speed = rospy.get_param(
self.controller_namespace + '/joint_speed', 1.0)
self.p_gain = rospy.get_param(self.controller_namespace + '/p_gain',
None)
self.i_gain = rospy.get_param(self.controller_namespace + '/i_gain',
None)
self.d_gain = rospy.get_param(self.controller_namespace + '/d_gain',
None)
self.torque_limit = rospy.get_param(
self.controller_namespace + '/joint_torque_limit', None)
self.torque_enable = rospy.get_param(
self.controller_namespace + '/torque_enable_init', False)
# Check params
self.__ensure_limits()
# Motor params from yaml
self.motor_id = rospy.get_param(self.controller_namespace +
'/motor/id')
self.initial_position_raw = rospy.get_param(self.controller_namespace +
'/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/max')
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace +
'/motor/acceleration')
else:
self.acceleration = None
# Flipped
self.flipped = self.min_angle_raw > self.max_angle_raw
# Base joint state msgs
self.joint_state = JointState(name=self.joint_name,
motor_ids=[self.motor_id])
# Dynamic reconfigure
self.reconfig_server = DynamicReconfServer(MotorMXParamsConfig,
self.process_params,
self.joint_name)
def publish_state(self):
sent_state = JointState()
sent_state.name = str(self._id)
sent_state.goal_pos = self.last_goal_in_radians
sent_state.velocity = int(
np.interp(self.lastvel,
(self._pololu_vel_min, self._pololu_vel_max), (0, 100)))
mutex.acquire()
pos = float(self.micro_maestro.getPosition(self._channel))
sent_state.is_moving = self.micro_maestro.isMoving(self._channel)
mutex.release()
sent_state.current_pos = self.usec2radians(pos)
self.state_motor_pub.publish(sent_state)
self.pub_plot_goal.publish(self.last_goal_in_radians)
self.pub_plot_current_pos.publish(sent_state.current_pos)
self.pub_plot_is_moving.publish(int(sent_state.is_moving))
def _BroadcastJointStateinfo_P8(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(lineParts)
if (partsCount < 4):
pass
try:
P1 = radians(float(lineParts[1]))#P1 = radians(float(lineParts[1]))
P2 = self.pan
P3 = 0#float(lineParts[3])
P4 = 0
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P8_MotorPublisher.publish(Motor_State)
self._pan_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "pan_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P8_JointPublisher.publish(Joint_State)
#rospy.logwarn(val)
except:
pass#rospy.logwarn("Unexpected error:pan_joint" + str(sys.exc_info()[0]))
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.master_id = self.declare_parameter('motor_master.id').value
self.master_initial_position_raw = self.declare_parameter(
'motor_master.init').value
self.master_min_angle_raw = self.declare_parameter(
'motor_master.min').value
self.master_max_angle_raw = self.declare_parameter(
'motor_master.max').value
self.slave_id = self.declare_parameter('motor_slave.id').value
self.slave_offset = self.declare_parameter(
'motor_slave.calibration_offset', 0).value
self.flipped = self.master_min_angle_raw > self.master_max_angle_raw
self.joint_state = JointState(
name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
def _BroadcastJointStateinfo_P2(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
cal = 0
A1 = (1023 + cal) - float(lineParts[1])
P1 = (A1 * 0.003070961)#(3.8-((float(lineParts[1])* 0.004597701)))# position
A2 = (1023 + cal) - float(lineParts[1])
P2 = (A2* 0.003070961)#float(lineParts[2])# target
P3 = float(lineParts[3])# current
P4 = float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
Joint_State = JointState()
Joint_State.name = "right_arm_lift_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
self._P2_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:2" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P2(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(partsCount)
if partsCount < 7:
pass
try:
off = 299
A1 = float(lineParts[1]) - off
P1 = 0 - ((A1 * 0.00174532925) - 0)
A2 = float(lineParts[2]) - off
P2 = 0 - ((A2 * 0.00174532925) - 0)
P3 = float(lineParts[3]) # current
P4 = 0 # float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
self._right_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "right_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now() # .stamprospy.Time.from_sec(state.timestamp)
self._P2_JointPublisher.publish(Joint_State)
# rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:right_arm_tilt_joint" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P3(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
#offset = Float(-1.57)
P1 = (float(lineParts[1])/1000)-0.6
P2 = self.right_lift #0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = 0#float(lineParts[3])
P4 = 0
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P3_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
self._left_lift_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_lift_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P3_JointPublisher.publish(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_lift_joint" + str(sys.exc_info()[0]))
def __init__(self, dxl_io, controller_namespace, port_namespace):
JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id')
self.gear_ratio = rospy.get_param(self.controller_namespace + '/gear_ratio')
self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max')
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration')
else:
self.acceleration = None
self.flipped = self.min_angle_raw > self.max_angle_raw
self.set_torque_enable(1)
self.dxl_io.set_acceleration_profile(self.motor_id, 20)
self.dxl_io.set_velocity_profile(self.motor_id, 200)
self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self):
self.axis_for_right = float(rospy.get_param(
'~axis_for_right',
0)) # if right calibrates first, this should be 0, else 1
self.wheel_track = float(rospy.get_param(
'~wheel_track', 0.285)) # m, distance between wheel centres
self.tyre_circumference = float(
rospy.get_param('~tyre_circumference', 0.341)
) # used to translate velocity commands in m/s into motor rpm
self.connect_on_startup = rospy.get_param('~connect_on_startup', False)
self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup',
False)
self.engage_on_startup = rospy.get_param('~engage_on_startup', False)
self.max_speed = rospy.get_param('~max_speed', 0.5)
self.max_angular = rospy.get_param('~max_angular', 1.0)
self.publish_current = rospy.get_param('~publish_current', True)
self.has_preroll = rospy.get_param('~use_preroll', False)
self.publish_current = rospy.get_param('~publish_current', True)
self.publish_odom = rospy.get_param('~publish_odom', True)
self.publish_tf = rospy.get_param('~publish_odom_tf', True)
self.odom_topic = rospy.get_param('~odom_topic', "odom")
self.odom_frame = rospy.get_param('~odom_frame', "odom")
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 20)
self.publish_joint_state = rospy.get_param('~publish_joint_state',
True)
self.joint_state_topic = rospy.get_param('~joint_state_topic',
"joint_state")
self.Calibrate_Axis = rospy.get_param('~Calibrate_Axis', 1)
self.motor_id = rospy.get_param('~motor_id', "0")
rospy.on_shutdown(self.terminate)
rospy.Service('connect_driver', std_srvs.srv.Trigger,
self.connect_driver)
rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
self.disconnect_driver)
rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
self.calibrate_motor)
rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
rospy.Service('release_motors', std_srvs.srv.Trigger,
self.release_motor)
self.vel_subscribe = rospy.Subscriber("/cmd_vel",
Twist,
self.cmd_vel_callback,
queue_size=2)
self.timer = rospy.Timer(
rospy.Duration(0.1),
self.timer_check) # stop motors if no cmd_vel received > 1second
if self.publish_current:
self.current_loop_count = 0
self.left_current_accumulator = 0.0
self.right_current_accumulator = 0.0
self.current_timer = rospy.Timer(
rospy.Duration(0.05), self.timer_current
) # publish motor currents at 10Hz, read at 50Hz
self.current_publisher_left = rospy.Publisher(
'odrive/left_current', Float64, queue_size=2)
self.current_publisher_right = rospy.Publisher(
'odrive/right_current', Float64, queue_size=2)
rospy.loginfo("ODrive will publish motor currents.")
if self.publish_odom:
rospy.Service('reset_odometry', std_srvs.srv.Trigger,
self.reset_odometry)
self.odom_publisher = rospy.Publisher(self.odom_topic,
Odometry,
queue_size=2)
# setup message
self.odom_msg = Odometry()
#print(dir(self.odom_msg))
self.odom_msg.header.frame_id = self.odom_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope
self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical
self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical
self.odom_msg.twist.twist.linear.y = 0.0 # no sideways
self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward
self.odom_msg.twist.twist.angular.x = 0.0 # or roll
self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw
# store current location to be updated.
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg = TransformStamped()
self.tf_msg.header.frame_id = self.odom_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.odom_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)),
self.timer_odometry)
if self.publish_joint_state:
self.joint_state_publisher = rospy.Publisher(
self.joint_state_topic, JointState, queue_size=2)
# setup message
self.joint_state_msg = JointState()
self.joint_state_msg.name = self.motor_id
self.state_subscriber = rospy.Subscriber(
'motor_states/%s' % self.motor_id, MotorState,
self.state_callback)
# setup message
self.state = MotorState()
self.RADIANS_PER_ENCODER_TICK = 2.0 * 3.1415926 / 4000.0
self.ENCODER_TICKS_PER_RADIAN = 1.0 / self.RADIANS_PER_ENCODER_TICK
#print(dir(self.odom_msg))
# self.joint_state_msg.header.frame_id = self.odom_frame
# self.joint_state_msg.child_frame_id = self.base_frame
if not self.connect_on_startup:
rospy.loginfo("ODrive node started, but not connected.")
return
if not self.connect_driver(None)[0]:
return # Failed to connect
if not self.calibrate_on_startup:
rospy.loginfo("ODrive node started and connected. Not calibrated.")
return
if not self.calibrate_motor(None)[0]:
return
if not self.engage_on_startup:
rospy.loginfo("ODrive connected and configured. Engage to drive.")
return
if not self.engage_motor(None)[0]:
return
rospy.loginfo("ODrive connected and configured. Ready to drive.")
def _HandleJoint_1_Command(self, data):
""" republish position. """
neck = (data)
self.gear_ratio = 0.3
new_pos = float(data.current_pos) * self.gear_ratio
new_goal = float(data.goal_pos) * self.gear_ratio
new_error = new_goal - new_pos
jointstate = JointState()
jointstate.name = ("head_tilt_mod_joint")
jointstate.motor_ids = (neck.motor_ids)
jointstate.motor_temps = (neck.motor_temps)
jointstate.goal_pos = (new_goal)
jointstate.current_pos = (new_pos)
jointstate.velocity = (neck.velocity)
jointstate.load = (neck.load)
jointstate.is_moving = (neck.is_moving)
jointstate.error = (new_error)
#rospy.loginfo(jointstate)
self._neckPublisher.publish(jointstate)
def __init__(self, dxl_io, controller_namespace, port_namespace):
self.running = False
self.dxl_io = dxl_io
self.controller_namespace = controller_namespace
self.port_namespace = port_namespace
self.joint_name = rospy.get_param(self.controller_namespace +
'/joint_name')
self.joint_speed = rospy.get_param(
self.controller_namespace + '/joint_speed', 1.0)
self.compliance_slope = rospy.get_param(
self.controller_namespace + '/joint_compliance_slope', None)
self.compliance_margin = rospy.get_param(
self.controller_namespace + '/joint_compliance_margin', None)
self.compliance_punch = rospy.get_param(
self.controller_namespace + '/joint_compliance_punch', None)
self.torque_limit = rospy.get_param(
self.controller_namespace + '/joint_torque_limit', None)
self.torque_enable = rospy.get_param(
self.controller_namespace + '/torque_enable_init', False)
self.__ensure_limits()
self.motor_id = rospy.get_param(self.controller_namespace +
'/motor/id')
self.initial_position_raw = rospy.get_param(self.controller_namespace +
'/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace +
'/motor/max')
if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
self.acceleration = rospy.get_param(self.controller_namespace +
'/motor/acceleration')
else:
self.acceleration = None
self.flipped = self.min_angle_raw > self.max_angle_raw
self.joint_state = JointState(name=self.joint_name,
motor_ids=[self.motor_id])
# PID control loop
self.enable_pid = rospy.get_param(
self.controller_namespace + '/enable_pid', False)
self.p_gain = rospy.get_param(self.controller_namespace + '/p_gain',
0.0)
self.i_gain = rospy.get_param(self.controller_namespace + '/i_gain',
0.0)
self.d_gain = rospy.get_param(self.controller_namespace + '/d_gain',
0.0)
self.i_clamp = rospy.get_param(self.controller_namespace + '/i_clamp',
None)
print[self.joint_name, self.enable_pid, self.p_gain, self.i_gain,
self.d_gain, self.i_clamp]
self.pid = PID(kp=self.p_gain,
ki=self.i_gain,
kd=self.d_gain,
i_clamp=self.i_clamp)
self.real_goal = 0.0
# Dynamic reconf server
self.reconfig_server = DynamicReconfServer(MotorParamsConfig,
self.process_params,
self.joint_name)
