Difference between pages "Tutorials" and "Joint Position Example"

(Difference between pages)
Jump to: navigation , search
(Running Examples Remove Not Used examples)
 
(/ * Edit move code walkthrough to same page */)
 
Line 1: Line 1:
 
__NOTOC__
 
__NOTOC__
 
<div class="title-block">
 
<div class="title-block">
<span style="font-size:120%;">'''This page contains a list of Tutorials you can use to access the basic functionality of the SDK layer of Intera'''</span>
+
 
 +
<span style="font-size:120%;">'''The joint position waypoints example demonstrates basic joint position control. A joint position waypoint is a configuration of the arm in joint space (ie. simultaneous joint angles for all of the arm's seven degrees of freedom). In this example, we enable the robot moving the specified limb in zero-g mode. Interacting with the arm's navigator buttons, we can record a sequence of joint position waypoints. Upon completion of recording, we will continuously command the limb loop through the recorded joint sequence.'''</span>
 +
 
 +
</div>
 +
 
 +
<div class="content-block">
 +
 
 +
== Introduction ==
 +
 
 +
This example demonstrates the usage of the position controller for simple joint position moves. The <code>main()</code> function calls the <code>record()</code> method which records the waypoints as desired by the user. Then, the <code>playback()</code> method is invoked which demonstrates a smooth motion playback through the recorded joint positions.
 +
<br />
 +
'''Interfaces - '''
 +
<div style="column-count:3;-moz-column-count:3;-webkit-column-count:3">
 +
* Limb.move_to_joint_positions()
 +
* Limb.set_joint_position_speed()
 +
* Limb.joint_angles()
 +
* Navigator.wheel()
 +
* Navigator.button_state()
 +
</div>
 +
 
 
</div>
 
</div>
  
 
<div class="content-block">
 
<div class="content-block">
  
== Running Examples ==
+
== Usage ==
 +
 
 +
Verify that the robot is enabled from an [[SDK Shell|SDK terminal session]], ex:
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_interface robot_enable.py
 +
</syntaxhighlight>
 +
 
 +
 
 +
Start the joint position waypoints example program, you can specify the joint position motion speed ratio <code>-s</code> or joint position accuracy(rad) <code>-a</code> on which we will run the example.
 +
 
 +
For example, run the example with 0.1 speed ratio and 0.1 accuracy:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_examples joint_position_waypoints.py -s 0.1 -a 0.1
 +
</syntaxhighlight>
 +
 
  
[[Running Examples Overview|Running Examples Overview]]
+
Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that [[Navigator | navigator]] wheel.
  
<div style="column-count:3;-moz-column-count:3;-webkit-column-count:3">
 
  
=== Examples ===
+
You will then be prompted with instructions for recording joint position waypoints.
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
Press Navigator 'OK/Wheel' button to record a new joint joint position waypoint.
 +
Press Navigator 'Rethink' button when finished recording waypoints to begin playback.
 +
</syntaxhighlight>
 +
 
 +
 
 +
Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that [[Navigator | navigator]] wheel.
 +
 
 +
 
 +
You will get the feedback that the joint position waypoint has been recorded:
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
Waypoint Recorded
 +
</syntaxhighlight>
 +
 
 +
 
 +
When done recording waypoints, pressing the [[Hardware_Components#Navigator | navigator's]] 'Rethink' button will begin playback.
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
[INFO] [WallTime: 1399571721.540300] Waypoint Playback Started
 +
  Press Ctrl-C to stop...
 +
</syntaxhighlight>
 +
 
 +
 
 +
The program will begin looping through the recorded joint positions. When the previous joint position command is fully achieved (within the accuracy threshold), the next recorded joint position will be commanded.
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
Waypoint playback loop #1
 +
Waypoint playback loop #2
 +
...
 +
</syntaxhighlight>
  
*[[Display Image Example]]
+
Pressing <code> Control-C </code> at any time will stop playback and exit the example.
*[[Gripper Example]]
 
*[[Gripper Cuff Control Example]]
 
*[[Head Movement Example]]
 
*[[IK Service Example]]
 
*[[Lights Blink Example]]
 
*[[Joint Position Example]]
 
*[[Joint Torque Springs Example]]
 
*[[Joint Trajectory Playback Example]]
 
*[[Simple Joint Trajectory Example]]
 
</div>
 
  
 
</div>
 
</div>
Line 30: Line 85:
 
<div class="content-block">
 
<div class="content-block">
  
== Using MoveIt! ==
+
== Arguments ==
  
[[MoveIt! Tutorial | MoveIt! Tutorial]]
+
'''Important Arguments:''' 
 +
 
 +
<code> -s </code> or <code> --speed </code>: The joint position motion speed ratio [0.0-1.0] (default:= 0.3)
 +
 
 +
<code> -a </code> or <code> --accuracy </code>: The threshold in Radians at which the current joint position command is considered successful before sending the next following joint position command. (default:= 0.008726646)
 +
 
 +
See the joint position waypoint's available arguments on the command line by passing joint_position_waypoints.py the <code>-h</code>, help argument:
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_examples joint_position_waypoints.py -h
 +
</syntaxhighlight>
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
usage: joint_position_waypoints.py [-h] [-s SPEED] [-a ACCURACY]
 +
 
 +
RSDK Joint Position Waypoints Example
 +
 
 +
    Records joint positions each time the navigator 'OK/wheel'
 +
    button is pressed.
 +
    Upon pressing the navigator 'Rethink' button, the recorded joint positions
 +
    will begin playing back in a loop.
 +
   
 +
 
 +
optional arguments:
 +
  -h, --help            show this help message and exit
 +
  -s SPEED, --speed SPEED
 +
                        joint position motion speed ratio [0.0-1.0] (default:=
 +
                        0.3)
 +
  -a ACCURACY, --accuracy ACCURACY
 +
                        joint position accuracy (rad) at which waypoints must
 +
                        achieve
 +
 
 +
</syntaxhighlight>
  
 
</div>
 
</div>
Line 38: Line 124:
 
<div class="content-block">
 
<div class="content-block">
  
== Using Gazebo ==
+
== Code Walkthrough ==  
 +
 
 +
Now, let's break down the code.
 +
 
 +
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
import argparse
 +
import sys
 +
 
 +
import rospy
 +
 
 +
import intera_interface
 +
</syntaxhighlight>
 +
 
 +
This imports the intera interface for accessing the limb and the gripper class.
 +
 
 +
<syntaxhighlight lang="python" line start="41" enclose="div">
 +
class Waypoints(object):
 +
    def __init__(self, speed, accuracy, limb="right"):
 +
        # Create intera_interface limb instance
 +
        self._arm = limb
 +
        self._limb = intera_interface.Limb(self._arm)
 +
 
 +
        # Parameters which will describe joint position moves
 +
        self._speed = speed
 +
        self._accuracy = accuracy
 +
 
 +
        # Recorded waypoints
 +
        self._waypoints = list()
 +
 
 +
        # Recording state
 +
        self._is_recording = False
 +
 
 +
        # Verify robot is enabled
 +
        print("Getting robot state... ")
 +
        self._rs = intera_interface.RobotEnable()
 +
        self._init_state = self._rs.state().enabled
 +
        print("Enabling robot... ")
 +
        self._rs.enable()
 +
 
 +
        # Create Navigator I/O
 +
        self._navigator_io = intera_interface.Navigator(self._arm)
 +
</syntaxhighlight>
 +
 
 +
An instance of the limb interface for the side under interest is created. An instance of the interface for the corresponding Navigator is also created. The robot is then enabled. It is important to note that the robot should be enabled in order to control the limb's movement.
 +
 
 +
<syntaxhighlight lang="python" line start="67" enclose="div">
 +
    def _record_waypoint(self, value):
 +
        """
 +
        Stores joint position waypoints
 +
 
 +
        Navigator 'OK/Wheel' button callback
 +
        """
 +
        if value != 'OFF':
 +
            print("Waypoint Recorded")
 +
            self._waypoints.append(self._limb.joint_angles())
 +
</syntaxhighlight>
 +
 
 +
The <code>joint_angles()</code> method retrieves the current joint angles and they are then appended to the <code>_waypoints</code> list.
 +
 
 +
<syntaxhighlight lang="python" line start="77" enclose="div">
 +
    def _stop_recording(self, value):
 +
        """
 +
        Sets is_recording to false
 +
 
 +
        Navigator 'Rethink' button callback
 +
        """
 +
        # On navigator Rethink button press, stop recording
 +
        if value != 'OFF':
 +
            print("Recording Stopped")
 +
            self._is_recording = False
 +
</syntaxhighlight>
 +
 
 +
This Rethink button's press event is captured.
 +
 
 +
<syntaxhighlight lang="python" line start="88" enclose="div">
 +
    def record(self):
 +
        """
 +
        Records joint position waypoints upon each Navigator 'OK/Wheel' button
 +
        press.
 +
        """
 +
        rospy.loginfo("Waypoint Recording Started")
 +
        print("Press Navigator 'OK/Wheel' button to record a new joint "
 +
        "joint position waypoint.")
 +
        print("Press Navigator 'Rethink' button when finished recording "
 +
              "waypoints to begin playback")
 +
        # Connect Navigator callbacks
 +
        # Navigator scroll wheel button press
 +
        ok_id = self._navigator.register_callback(self._record_waypoint, 'right_button_ok')
 +
        # Navigator Rethink button press
 +
        show_id = self._navigator.register_callback(self._stop_recording, 'right_button_show')
 +
 
 +
        # Set recording flag
 +
        self._is_recording = True
 +
 
 +
        # Loop until waypoints are done being recorded ('Rethink' Button Press)
 +
        while not rospy.is_shutdown() and self._is_recording:
 +
            rospy.sleep(1.0)
 +
 
 +
        # We are now done with the navigator callbacks, disconnecting them
 +
        self._navigator.deregister_callback(ok_id)
 +
        self._navigator.deregister_callback(show_id)
 +
</syntaxhighlight>
 +
 
 +
The navigator 'OK/Wheel' button invokes the <code>_record_waypoint()</code> method on a press event, which records the current joint positions as explained above. Similarly, the navigator 'Rethink' button invokes the <code>_stop_recording</code> method on a press event, which sets the <code>is_recording</code> variable to false.
 +
 
 +
<syntaxhighlight lang="python" line start="115" enclose="div">
 +
    def playback(self):
 +
        """
 +
        Loops playback of recorded joint position waypoints until program is
 +
        exited
 +
        """
 +
        rospy.sleep(1.0)
 +
 
 +
        rospy.loginfo("Waypoint Playback Started")
 +
        print("  Press Ctrl-C to stop...")
 +
 
 +
        # Set joint position speed ratio for execution
 +
        self._limb.set_joint_position_speed(self._speed)
 +
 
 +
        # Loop until program is exited
 +
        loop = 0
 +
        while not rospy.is_shutdown():
 +
            loop += 1
 +
            print("Waypoint playback loop #%d " % (loop,))
 +
            for waypoint in self._waypoints:
 +
                if rospy.is_shutdown():
 +
                    break
 +
                self._limb.move_to_joint_positions(waypoint, timeout=20.0,
 +
                                                  threshold=self._accuracy)
 +
            # Sleep for a few seconds between playback loops
 +
            rospy.sleep(3.0)
 +
 
 +
        # Set joint position speed back to default
 +
        self._limb.set_joint_position_speed(0.3)
 +
</syntaxhighlight>
 +
 
 +
The <code>set_joint_position_speed()</code> method sets the ratio of max joint speed to use during joint position moves. The method <code>move_to_joint_positions()</code>, moves the joints to the commanded position. It is important to note that there is not trajectory planning involved here. Instead, this is passed onto a low pass filter and the intermediate positions between the start and goal positions are obtained. They are then published as a joint command message using the <code>set_joint_positions()</code> method. Thus, all the waypoints that were stored are visited along a smooth trajectory.
 +
 
 +
<syntaxhighlight lang="python" line start="144" enclose="div">
 +
    def clean_shutdown(self):
 +
        print("\nExiting example...")
 +
        if not self._init_state:
 +
            print("Disabling robot...")
 +
            self._rs.disable()
 +
        return True
 +
</syntaxhighlight>
 +
 
 +
On shutdown, the robot is sent to its initial state that was captured.
 +
 
 +
<syntaxhighlight lang="python" line start="152" enclose="div">
 +
def main():
 +
    """RSDK Joint Position Waypoints Example
 +
 
 +
    Records joint positions each time the navigator 'OK/wheel'
 +
    button is pressed.
 +
    Upon pressing the navigator 'Rethink' button, the recorded joint positions
 +
    will begin playing back in a loop.
 +
    """
 +
    arg_fmt = argparse.RawDescriptionHelpFormatter
 +
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
 +
                                    description=main.__doc__)
 +
    parser.add_argument(
 +
        '-s', '--speed', default=0.3, type=float,
 +
        help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
 +
    )
 +
    parser.add_argument(
 +
        '-a', '--accuracy',
 +
        default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
 +
        help='joint position accuracy (rad) at which waypoints must achieve'
 +
    )
 +
    args = parser.parse_args(rospy.myargv()[1:])
 +
</syntaxhighlight>
 +
 
 +
The speed and accuracy on which the example is to be demonstrated is captured from the command line arguments.
 +
 
 +
<syntaxhighlight lang="python" line start="178" enclose="div">
 +
    print("Initializing node... ")
 +
    rospy.init_node("rsdk_joint_position_waypoints")
 +
 
 +
    waypoints = Waypoints(args.speed, args.accuracy)
 +
 
 +
    # Register clean shutdown
 +
    rospy.on_shutdown(waypoints.clean_shutdown)
 +
 
 +
    # Begin example program
 +
    waypoints.record()
 +
    waypoints.playback()
 +
 
 +
if __name__ == '__main__':
 +
    main()
 +
</syntaxhighlight>
  
Gazebo Tutorial
+
The node is initialized and an instance of the <code>Waypoints</code> class is created. The user defined waypoints are recorded using the <code>record()</code> method and are played back using the <code>playback()</code> method as explained above.
  
 
</div>
 
</div>

Revision as of 12:35, 24 November 2016

The joint position waypoints example demonstrates basic joint position control. A joint position waypoint is a configuration of the arm in joint space (ie. simultaneous joint angles for all of the arm's seven degrees of freedom). In this example, we enable the robot moving the specified limb in zero-g mode. Interacting with the arm's navigator buttons, we can record a sequence of joint position waypoints. Upon completion of recording, we will continuously command the limb loop through the recorded joint sequence.

Introduction

This example demonstrates the usage of the position controller for simple joint position moves. The main() function calls the record() method which records the waypoints as desired by the user. Then, the playback() method is invoked which demonstrates a smooth motion playback through the recorded joint positions.
Interfaces -

  • Limb.move_to_joint_positions()
  • Limb.set_joint_position_speed()
  • Limb.joint_angles()
  • Navigator.wheel()
  • Navigator.button_state()

Usage

Verify that the robot is enabled from an SDK terminal session, ex:

$ rosrun intera_interface robot_enable.py


Start the joint position waypoints example program, you can specify the joint position motion speed ratio -s or joint position accuracy(rad) -a on which we will run the example.

For example, run the example with 0.1 speed ratio and 0.1 accuracy:

$ rosrun intera_examples joint_position_waypoints.py -s 0.1 -a 0.1


Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that navigator wheel.


You will then be prompted with instructions for recording joint position waypoints.

Press Navigator 'OK/Wheel' button to record a new joint joint position waypoint.
Press Navigator 'Rethink' button when finished recording waypoints to begin playback.


Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that navigator wheel.


You will get the feedback that the joint position waypoint has been recorded:

Waypoint Recorded


When done recording waypoints, pressing the navigator's 'Rethink' button will begin playback.

[INFO] [WallTime: 1399571721.540300] Waypoint Playback Started
  Press Ctrl-C to stop...


The program will begin looping through the recorded joint positions. When the previous joint position command is fully achieved (within the accuracy threshold), the next recorded joint position will be commanded.

Waypoint playback loop #1
Waypoint playback loop #2
...

Pressing Control-C at any time will stop playback and exit the example.

Arguments

Important Arguments:

-s or --speed : The joint position motion speed ratio [0.0-1.0] (default:= 0.3)

-a or --accuracy : The threshold in Radians at which the current joint position command is considered successful before sending the next following joint position command. (default:= 0.008726646)

See the joint position waypoint's available arguments on the command line by passing joint_position_waypoints.py the -h, help argument:

$ rosrun intera_examples joint_position_waypoints.py -h
usage: joint_position_waypoints.py [-h] [-s SPEED] [-a ACCURACY]

RSDK Joint Position Waypoints Example

    Records joint positions each time the navigator 'OK/wheel'
    button is pressed.
    Upon pressing the navigator 'Rethink' button, the recorded joint positions
    will begin playing back in a loop.
    

optional arguments:
  -h, --help            show this help message and exit
  -s SPEED, --speed SPEED
                        joint position motion speed ratio [0.0-1.0] (default:=
                        0.3)
  -a ACCURACY, --accuracy ACCURACY
                        joint position accuracy (rad) at which waypoints must
                        achieve

Code Walkthrough

Now, let's break down the code.

33 import argparse
34 import sys
35 
36 import rospy
37 
38 import intera_interface

This imports the intera interface for accessing the limb and the gripper class.

41 class Waypoints(object):
42     def __init__(self, speed, accuracy, limb="right"):
43         # Create intera_interface limb instance
44         self._arm = limb
45         self._limb = intera_interface.Limb(self._arm)
46 
47         # Parameters which will describe joint position moves
48         self._speed = speed
49         self._accuracy = accuracy
50 
51         # Recorded waypoints
52         self._waypoints = list()
53 
54         # Recording state
55         self._is_recording = False
56 
57         # Verify robot is enabled
58         print("Getting robot state... ")
59         self._rs = intera_interface.RobotEnable()
60         self._init_state = self._rs.state().enabled
61         print("Enabling robot... ")
62         self._rs.enable()
63 
64         # Create Navigator I/O
65         self._navigator_io = intera_interface.Navigator(self._arm)

An instance of the limb interface for the side under interest is created. An instance of the interface for the corresponding Navigator is also created. The robot is then enabled. It is important to note that the robot should be enabled in order to control the limb's movement.

67     def _record_waypoint(self, value):
68         """
69         Stores joint position waypoints
70 
71         Navigator 'OK/Wheel' button callback
72         """
73         if value != 'OFF':
74             print("Waypoint Recorded")
75             self._waypoints.append(self._limb.joint_angles())

The joint_angles() method retrieves the current joint angles and they are then appended to the _waypoints list.

77     def _stop_recording(self, value):
78         """
79         Sets is_recording to false
80 
81         Navigator 'Rethink' button callback
82         """
83         # On navigator Rethink button press, stop recording
84         if value != 'OFF':
85             print("Recording Stopped")
86             self._is_recording = False

This Rethink button's press event is captured.

 88     def record(self):
 89         """
 90         Records joint position waypoints upon each Navigator 'OK/Wheel' button
 91         press.
 92         """
 93         rospy.loginfo("Waypoint Recording Started")
 94         print("Press Navigator 'OK/Wheel' button to record a new joint "
 95         "joint position waypoint.")
 96         print("Press Navigator 'Rethink' button when finished recording "
 97               "waypoints to begin playback")
 98         # Connect Navigator callbacks
 99         # Navigator scroll wheel button press
100         ok_id = self._navigator.register_callback(self._record_waypoint, 'right_button_ok')
101         # Navigator Rethink button press
102         show_id = self._navigator.register_callback(self._stop_recording, 'right_button_show')
103 
104         # Set recording flag
105         self._is_recording = True
106 
107         # Loop until waypoints are done being recorded ('Rethink' Button Press)
108         while not rospy.is_shutdown() and self._is_recording:
109             rospy.sleep(1.0)
110 
111         # We are now done with the navigator callbacks, disconnecting them
112         self._navigator.deregister_callback(ok_id)
113         self._navigator.deregister_callback(show_id)

The navigator 'OK/Wheel' button invokes the _record_waypoint() method on a press event, which records the current joint positions as explained above. Similarly, the navigator 'Rethink' button invokes the _stop_recording method on a press event, which sets the is_recording variable to false.

115     def playback(self):
116         """
117         Loops playback of recorded joint position waypoints until program is
118         exited
119         """
120         rospy.sleep(1.0)
121 
122         rospy.loginfo("Waypoint Playback Started")
123         print("  Press Ctrl-C to stop...")
124 
125         # Set joint position speed ratio for execution
126         self._limb.set_joint_position_speed(self._speed)
127 
128         # Loop until program is exited
129         loop = 0
130         while not rospy.is_shutdown():
131             loop += 1
132             print("Waypoint playback loop #%d " % (loop,))
133             for waypoint in self._waypoints:
134                 if rospy.is_shutdown():
135                     break
136                 self._limb.move_to_joint_positions(waypoint, timeout=20.0,
137                                                    threshold=self._accuracy)
138             # Sleep for a few seconds between playback loops
139             rospy.sleep(3.0)
140 
141         # Set joint position speed back to default
142         self._limb.set_joint_position_speed(0.3)

The set_joint_position_speed() method sets the ratio of max joint speed to use during joint position moves. The method move_to_joint_positions(), moves the joints to the commanded position. It is important to note that there is not trajectory planning involved here. Instead, this is passed onto a low pass filter and the intermediate positions between the start and goal positions are obtained. They are then published as a joint command message using the set_joint_positions() method. Thus, all the waypoints that were stored are visited along a smooth trajectory.

144     def clean_shutdown(self):
145         print("\nExiting example...")
146         if not self._init_state:
147             print("Disabling robot...")
148             self._rs.disable()
149         return True

On shutdown, the robot is sent to its initial state that was captured.

152 def main():
153     """RSDK Joint Position Waypoints Example
154 
155     Records joint positions each time the navigator 'OK/wheel'
156     button is pressed.
157     Upon pressing the navigator 'Rethink' button, the recorded joint positions
158     will begin playing back in a loop.
159     """
160     arg_fmt = argparse.RawDescriptionHelpFormatter
161     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
162                                      description=main.__doc__)
163     parser.add_argument(
164         '-s', '--speed', default=0.3, type=float,
165         help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
166     )
167     parser.add_argument(
168         '-a', '--accuracy',
169         default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
170         help='joint position accuracy (rad) at which waypoints must achieve'
171     )
172     args = parser.parse_args(rospy.myargv()[1:])

The speed and accuracy on which the example is to be demonstrated is captured from the command line arguments.

178     print("Initializing node... ")
179     rospy.init_node("rsdk_joint_position_waypoints")
180 
181     waypoints = Waypoints(args.speed, args.accuracy)
182 
183     # Register clean shutdown
184     rospy.on_shutdown(waypoints.clean_shutdown)
185 
186     # Begin example program
187     waypoints.record()
188     waypoints.playback()
189 
190 if __name__ == '__main__':
191     main()

The node is initialized and an instance of the Waypoints class is created. The user defined waypoints are recorded using the record() method and are played back using the playback() method as explained above.