control_effort_wrist.py

Go to the documentation of this file.

#! /usr/bin/env python
# coding: utf-8

import math
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64


class PIDController(object):
    def __init__(self, p_gain, i_gain, d_gain):

        self._p_gain = p_gain
        self._i_gain = i_gain
        self._d_gain = d_gain

        self._error_1 = 0.0
        self._error_2 = 0.0
        self._output = 0.0

    def update(self, current, target):
        error = target - current

        delta_output = self._p_gain * (error - self._error_1)
        delta_output += self._i_gain * (error)
        delta_output += self._d_gain * (error - 2*self._error_1 + self._error_2)

        self._output += delta_output

        self._error_2 = self._error_1
        self._error_1 = error

        return self._output


def stop():
    pub_wrist_current.publish(0.0)
    print("TORQUE_OFF")

joint_state = JointState()
def joint_state_callback(msg):
    global joint_state
    joint_state = msg

def main():
    global joint_state

    # Set P and D gain to avoid the wrist oscillation.
    pid_controller = PIDController(0.5, 0.0, 3.0)

    r = rospy.Rate(60)
    start_time = rospy.Time.now().secs
    target_angle = math.radians(90)
    while not rospy.is_shutdown():
        if len(joint_state.position) < 7:  # Wrist is the 7th joint.
            continue

        wrist_angle = joint_state.position[6]

        # Switch the target angle every 5 seconds.
        present_time = rospy.Time.now().secs
        if present_time - start_time > 5:
            start_time = present_time
            target_angle *= -1.0

        target_effort = pid_controller.update(wrist_angle, target_angle)
        pub_wrist_current.publish(target_effort)
        r.sleep()


if __name__ == '__main__':
    rospy.init_node("control_effort_wrist")

    sub_joint_state = rospy.Subscriber(
        "/sciurus17/controller2/joint_states",
        JointState,
        joint_state_callback,
        queue_size=1)

    pub_wrist_current = rospy.Publisher(
        "/sciurus17/controller2/left_wrist_controller/command",
        Float64,
        queue_size=1)

    rospy.on_shutdown(stop)

    try:
        if not rospy.is_shutdown():
            main()
    except rospy.ROSInterruptException:
        pass