Control Arm

control_arm.py

Overview

control_arm.py is a closed-loop ActionLib server that positions the TIAGo manipulator to a target joint angle (in degrees). It applies a proportional control law on the arm’s angular velocity until the on-board encoder reports the target is reached within tolerance, or a timeout/pre-emption occurs.

Interfaces (strongly-typed, stateful)

Direction

Name / Topic

Type

Purpose

Subscribe

/{robot}/encoder_arm

std_msgs/Int32

Current encoder position in degrees

Publish

/{robot}/cmd_vel/arm

geometry_msgs/Twist

Desired angular velocity (angular.z only)

Action server

/{robot}/arm_control

tiago1/ArmControlAction

Goal: degree (int) Feedback: joint status lists Result: success (bool)

Contract

Pre-conditions

  • Encoder publishes valid degree readings on /encoder_arm.

  • Action server is started before accepting goals.

Post-conditions

  • On success: publishes zero velocity, sets Action to succeeded.

  • On timeout or encoder never reaches target: publishes zero velocity, sets aborted.

  • On pre-emption: stops control, sets pre-empted.

  • Feedback published at 10 Hz during execution.

Invariants

  • Loop frequency = 10 Hz ± ROS scheduler jitter.

  • Velocity obeys ω = k_p · error with k_p constant.

Tuning knobs

  • TOLERANCE = 5 °

  • TIMEOUT = 10 s

  • Kₚ = 0.01

class control_arm.ControlArmServer[source]

Bases: object

ActionLib server exposing the arm_control namespace with a proportional joint controller.

Variables

encoder_valint

Latest encoder reading (degrees).

cmd_pubrospy.Publisher

Publishes Twist on /cmd_vel/arm to drive angular velocity.

serveractionlib.SimpleActionServer

Handles ArmControlAction goals, feedback, and results.

  1. Read robot_number from CLI.

  2. Initialise ROS node <robot>_control_arm_node.

  3. Advertise /cmd_vel/arm and subscribe to /encoder_arm.

  4. Set up and start the ActionLib server on /arm_control.

encoder_callback(msg: std_msgs.msg.Int32) → None[source]

Cache the most recent encoder position.

Parameters:

msg (std_msgs.msg.Int32) – Encoder reading in degrees.

execute_cb(goal: tiago1.msg.ArmControlAction.Goal) → None[source]

Execute a proportional control loop to reach goal.degree.

Parameters:

goal (ArmControlAction.Goal) – Contains degree target angle.