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)