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 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, 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, 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.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, 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 __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, 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 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 _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 __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 _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, *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 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 __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 _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, *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_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):
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 _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]))
# BEGIN ALL
import math
import rospy
from std_msgs.msg import Float64
from dynamixel_msgs.msg import JointState
from embedded_control.msg import StartEnd
import thread
from dynamixel_msgs.msg import MotorStateList
import gui_lidar
LOOP_RATE = 10 # In Hz
# ROTATION_ANGLE = math.pi/2 # In rad
# ERROR_THRESH = 0.3 # In rad
MOTOR_SPEED = 0.3
feedback = JointState()
#Callback
def feedback_callback(msg):
global feedback
feedback = msg
def speed_callback(msg):
global MOTOR_SPEED
MOTOR_SPEED = msg.data
def scan():
global feedback
def __init__(self):
global x, y, z, t1, t2, t3, t4, t5, wrist_arg, M_PI
rospy.init_node('test_arm', anonymous=True)
self.tilt1_pub = rospy.Publisher('tilt1_controller/command',
Float64,
queue_size=1000)
self.tilt2_pub = rospy.Publisher('tilt2_controller/command',
Float64,
queue_size=1000)
self.tilt3_pub = rospy.Publisher('tilt3_controller/command',
Float64,
queue_size=1000)
self.tilt4_pub = rospy.Publisher('tilt4_controller/command',
Float64,
queue_size=1000)
self.tilt5_pub = 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.tilt1_state = JointState()
self.tilt2_state = JointState()
self.tilt3_state = JointState()
self.tilt4_state = JointState()
self.tilt5_state = JointState()
self.tilt_moving = [0, 1, 1, 1, 1, 1]
self.rate = rospy.Rate(100)
#描画サイズの定義
CampusSizeX = 0.20
CampusSizeY = 0.14
CampusOffsetX = -0.10
CampusOffsetY = +0.14
FontSize = 0.04
#高さ設定
height_down = 0.100
height_up = height_down + 0.020
#画面描画用の設定
Ratio = 2000
ImageSizeX = int(CampusSizeX * Ratio)
ImageSizeY = int(CampusSizeY * Ratio)
img = np.zeros((ImageSizeY, ImageSizeX, 3), np.uint8)
img[:, :, :] = (255, 255, 255)
color = (0, 0, 0)
thickness = 2
cv2.namedWindow("img")
#筆跡データ読込（ファイル名は固定の暫定対応）
data = np.load("/home/ubuntu/moji.npy")
########################################
#メイン処理
z = height_up
x0 = 0
y0 = 0
####################
#一筆分のデータ取得
for data0 in data:
#NULLデータの場合は処理スキップ
if data0[0][0] == -100:
continue
print data0
####################
#一筆分のデータ取得
for data1 in data0:
#NULLデータの場合は処理スキップ
if data1[0] == -100:
continue
print data1
#座標算出
x = (data1[0] / 100.0) * FontSize + CampusOffsetX
y = -(data1[1] / 100.0) * FontSize + CampusOffsetY + FontSize
#移動量が小さい場合はスキップ
if abs(x - x0) + abs(y - y0) < 0.002:
continue
#アームを動かす
self.trans_pos()
self.move_arm([0, t1, t2, t3, t4, t5], 1)
#高さがdownモードの時は、画面にもライン描画
if z == height_down:
#描画用座標算出してラインを引く
lx0 = int((x0 - CampusOffsetX) * Ratio)
lx = int((x - CampusOffsetX) * Ratio)
ly0 = ImageSizeY - int((y0 - CampusOffsetY) * Ratio)
ly = ImageSizeY - int((y - CampusOffsetY) * Ratio)
cv2.line(img, (lx0, ly0), (lx, ly), color, thickness)
cv2.imshow("img", img)
cv2.waitKey(1)
print(lx0, lx, ly0, ly)
#高さがupモードの時は、x,y移動後にペンを下げる
if z == height_up:
sleep(1.0)
z = height_down
self.trans_pos()
self.move_arm([0, t1, t2, t3, t4, t5], 10)
#参照用のx,y座標を退避
x0 = x
y0 = y
#一筆分が終了したら、ペンを上げる
sleep(1.0)
z = height_up
self.trans_pos()
self.move_arm([0, t1, t2, t3, t4, t5], 10)
#処理が終ったら、キー入力を待って終了
print("Please turn key to finish.")
cv2.waitKey(0)
exit()
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)
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.")
