def __init__(self, limb, reconfig_server, rate=100.0):
self._dyn = reconfig_server
self._ns = 'robot/limb/' + limb + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
self._limb = baxter_interface.Limb(limb)
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments and dynamic reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._goal_error = dict()
self._error_threshold = dict()
self._dflt_vel = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._limb.joint_names():
self._pid[joint] = baxter_control.PID()
# Set joint state publishing to specified control rate
self._pub_rate = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._pub_rate.publish(self._control_rate)
def __init__(self, limb, reconfig_server, rate=100.0, mode='position_w_id'):
self._dyn = reconfig_server
self._ns = 'robot/limb/' + limb
self._fjt_ns = self._ns + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False
)
self._action_name = rospy.get_name()
self._limb = baxter_interface.Limb(limb)
self._enable = baxter_interface.RobotEnable()
self._name = limb
self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb, ))
self._cuff.state_changed.connect(self._cuff_cb)
# Verify joint control mode
self._mode = mode
if (self._mode != 'position' and self._mode != 'position_w_id' and self._mode != 'velocity'):
rospy.logerr(
"%s: Action Server Creation Failed - "
"Provided Invalid Joint Control Mode '%s' (Options: "
"'position_w_id', 'position', 'velocity')" % (
self._action_name,
self._mode,
)
)
return
self._server.start()
self._alive = True
self._cuff_state = False
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic
# reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._limb.joint_names():
self._pid[joint] = baxter_control.PID()
# Create our spline coefficients
self._coeff = [None] * len(self._limb.joint_names())
# Set joint state publishing to specified control rate
self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate', UInt16, queue_size=10)
self._pub_rate.publish(self._control_rate)
self._pub_ff_cmd = rospy.Publisher(
self._ns + '/inverse_dynamics_command', JointTrajectoryPoint, tcp_nodelay=True, queue_size=1
)
def _start_controller(self):
control_rate = rospy.Rate(self._control_rate)
print "Hold Baxter's cuff sensors to start..."
# Waits until ctrl+c or someone holds the cuff
while not rospy.is_shutdown():
if not self.robot_is_enabled():
return
if self._cuff_state:
break
control_rate.sleep()
# Create PID controllers for all joints
# Only the stiff joints are going to be controlled, the others will get torque ZERO.
self._pid = dict()
for joint in self.stiff_joints_names:
self._pid[joint] = baxter_control.PID()
self._pid[joint].set_kp(self._gains[joint]['kp'])
self._pid[joint].set_ki(self._gains[joint]['ki'])
self._pid[joint].set_kd(self._gains[joint]['kd'])
self._pid[joint].initialize()
self._enable_pub = rospy.Publisher('robot/set_super_enable',
Bool,
queue_size=10)
print "Baxter is under control..."
# Does the control magic :D
while not rospy.is_shutdown():
if not self.robot_is_enabled():
rospy.logerr("ZeroG problem with _start_controller...")
break
if not self._cuff_state:
print "The user released the cuff sensor...exiting!"
break
# self._disable_original_zero_g.publish()
self._update_command()
control_rate.sleep()
print "The end!"