diff --git a/follow_joint_trajectory.md b/follow_joint_trajectory.md
index 99a6c71..6a638ab 100644
--- a/follow_joint_trajectory.md
+++ b/follow_joint_trajectory.md
@@ -1,6 +1,6 @@
## FollowJointTrajectory Commands
-Stretch ROS driver offers a [`FollowJointTrajectory`](http://docs.ros.org/en/api/control_msgs/html/action/FollowJointTrajectory.html) action service for its arm. Within this tutorial we will have a simple FollowJointTrajectory commands sent to a Stretch robot to execute.
+Stretch ROS driver offers a [`FollowJointTrajectory`](http://docs.ros.org/en/api/control_msgs/html/action/FollowJointTrajectory.html) action service for its arm. Within this tutorial we will have a simple FollowJointTrajectory command sent to a Stretch robot to execute.
## Stow Command Example
@@ -21,7 +21,7 @@ cd catkin_ws/src/stretch_ros_turotials/src/
python3 stow_command.py
```
-This will sent FollowJointTrajectory commands to stow Stretch's arm.
+This will send a FollowJointTrajectory command to stow Stretch's arm.
### The Code
```python
@@ -138,3 +138,121 @@ if __name__ == '__main__':
```
Initialize the `StowCommand()` class and set it to *node* and run the `main()` function.
+
+
+## Multipoint Command Example
+
+
+ 
+
+
+Begin by running `roscore` in a terminal. Then set the ros parameter to *position* mode by running the following commands in a new terminal.
+
+```bash
+rosparam set /stretch_driver/mode "position"
+roslaunch stretch_core stretch_driver.launch
+```
+
+In a new terminal type the following commands.
+
+```bash
+cd catkin_ws/src/stretch_ros_turotials/src/
+python3 multipoint_command.py
+```
+
+This will send a list of JointTrajectoryPoint's to move Stretch's arm.
+
+### The Code
+```python
+#!/usr/bin/env python3
+
+import rospy
+import time
+from control_msgs.msg import FollowJointTrajectoryGoal
+from trajectory_msgs.msg import JointTrajectoryPoint
+import hello_helpers.hello_misc as hm
+
+class MultiPointCommand(hm.HelloNode):
+
+ def __init__(self):
+ hm.HelloNode.__init__(self)
+
+ def issue_multipoint_command(self):
+ point0 = JointTrajectoryPoint()
+ point0.positions = [0.2, 0.0, 3.4]
+ point0.velocities = [0.2, 0.2, 2.5]
+ point0.accelerations = [1.0, 1.0, 3.5]
+
+ point1 = JointTrajectoryPoint()
+ point1.positions = [0.3, 0.1, 2.0]
+
+ point2 = JointTrajectoryPoint()
+ point2.positions = [0.5, 0.2, -1.0]
+
+ point3 = JointTrajectoryPoint()
+ point3.positions = [0.6, 0.3, 0.0]
+
+ point4 = JointTrajectoryPoint()
+ point4.positions = [0.8, 0.2, 1.0]
+
+ point5 = JointTrajectoryPoint()
+ point5.positions = [0.5, 0.1, 0.0]
+
+ trajectory_goal = FollowJointTrajectoryGoal()
+ trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
+ trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
+ trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
+ trajectory_goal.trajectory.header.frame_id = 'base_link'
+
+ self.trajectory_client.send_goal(trajectory_goal)
+ rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
+ self.trajectory_client.wait_for_result()
+
+ def main(self):
+ hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False)
+ rospy.loginfo('issuing multipoint command...')
+ self.issue_multipoint_command()
+ time.sleep(2)
+
+
+if __name__ == '__main__':
+ try:
+ node = MultiPointCommand()
+ node.main()
+ except KeyboardInterrupt:
+ rospy.loginfo('interrupt received, so shutting down')
+
+```
+
+### The Code Explained.
+Seeing that there are similarities between the multipoint and stow command nodes, we will only breakdown the different components of the multipoint_command node.
+
+```python
+point0 = JointTrajectoryPoint()
+point0.positions = [0.2, 0.0, 3.4]
+```
+Sett *point0* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively.
+
+
+
+```python
+point0.velocities = [0.2, 0.2, 2.5]
+```
+Provide desired velocity of the lift (m/s), wrist extension (m/s), and wrist yaw (rad/s) for *point0*.
+
+```python
+point0.accelerations = [1.0, 1.0, 3.5]
+```
+Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2), and wrist yaw (rad/s^2).
+
+**IMPORTANT NOTE**: The lift and wrist extension can only go up to 0.2 m/s. If you do not provide any velocities or accelerations for the lift or wrist extension, then they go to their default values. However, the Velocity and Acceleration of the wrist yaw will stay the same from the previous value unless updated.
+
+
+```python
+trajectory_goal = FollowJointTrajectoryGoal()
+trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
+trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
+trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
+trajectory_goal.trajectory.header.frame_id = 'base_link'
+```
+Set *trajectory_goal* as a `FollowJointTrajectoryGoal` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by a list of the 6 points. Specify the coordinate frame that we want (base_link) and set the time to be now.
diff --git a/images/multipoint.gif b/images/multipoint.gif
new file mode 100644
index 0000000..f67738a
Binary files /dev/null and b/images/multipoint.gif differ
diff --git a/src/multipoint_command.py b/src/multipoint_command.py
index 5c0e27d..aff3ebd 100644
--- a/src/multipoint_command.py
+++ b/src/multipoint_command.py
@@ -1,66 +1,97 @@
#!/usr/bin/env python3
+# Every python controller needs these lines
import rospy
+import time
+
+# Import the FollowJointTrajectoryGoal from the control_msgs.msg package to
+# control the Stretch robot.
from control_msgs.msg import FollowJointTrajectoryGoal
+
+# Import JointTrajectoryPoint from the trajectory_msgs package to define
+# robot trajectories.
from trajectory_msgs.msg import JointTrajectoryPoint
+
+# Import hello_misc script for handling trajecotry goals with an action client.
import hello_helpers.hello_misc as hm
-import time
class MultiPointCommand(hm.HelloNode):
+ # Initialize the inhereted hm.Hellonode class.
def __init__(self):
hm.HelloNode.__init__(self)
def issue_multipoint_command(self):
+ # Set point0 as a JointTrajectoryPoint().
point0 = JointTrajectoryPoint()
+
+ # Provide desired positions of lift, wrist extension, and yaw of
+ # the wrist (in meters).
point0.positions = [0.2, 0.0, 3.4]
- point0.velocities = [0.2]
- point0.accelerations = [1.0]
+ # Provide desired velocity of the lift (m/s), wrist extension (m/s),
+ # and wrist yaw (rad/s).
+ # IMPORTANT NOTE: The lift and wrist extension can only go up to 0.2 m/s!
+ point0.velocities = [0.2, 0.2, 2.5]
+
+ # Provide desired velocity of the lift (m/s^2), wrist extension (m/s^2),
+ # and wrist yaw (rad/s^2).
+ point0.accelerations = [1.0, 1.0, 3.5]
+
+ # Set positions for the following 5 trajectory points.
+ # IMPORTANT NOTE: If you do not provide any velocities or accelerations for the lift
+ # or wrist extension, then they go to their default values. However, the
+ # Velocity and Acceleration of the wrist yaw will stay the same from the
+ # previous value unless updated.
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
- point1.velocities = [0.1]
- point1.accelerations = [-1.0]
point2 = JointTrajectoryPoint()
point2.positions = [0.5, 0.2, -1.0]
- point2.velocities = [0.08]
- point2.accelerations = [-1.0]
point3 = JointTrajectoryPoint()
point3.positions = [0.6, 0.3, 0.0]
- point3.velocities = [0.1]
- point3.accelerations = [1.0]
point4 = JointTrajectoryPoint()
point4.positions = [0.8, 0.2, 1.0]
- point4.velocities = [0.2]
- point4.accelerations = [1]
point5 = JointTrajectoryPoint()
point5.positions = [0.5, 0.1, 0.0]
-
+ # Set trajectory_goal as a FollowJointTrajectoryGoal and define
+ # the joint names as a list.
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
+
+ # Then trajectory_goal.trajectory.points is defined by a list of the joint
+ # trajectory points
trajectory_goal.trajectory.points = [point0, point1, point2, point3, point4, point5]
+
+ # Specify the coordinate frame that we want (base_link) and set the time to be now.
trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
trajectory_goal.trajectory.header.frame_id = 'base_link'
+ # Make the action call and send the goal. The last line of code waits
+ # for the result before it exits the python script.
self.trajectory_client.send_goal(trajectory_goal)
rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))
self.trajectory_client.wait_for_result()
-
+ # Create a funcion, main(), to do all of the setup the hm.HelloNode class
+ # and issue the stow command.
def main(self):
+ # The arguments of the main function of the hm.HelloNode class are the
+ # node_name, node topic namespace, and boolean (default value is true).
hm.HelloNode.main(self, 'multipoint_command', 'multipoint_command', wait_for_first_pointcloud=False)
rospy.loginfo('issuing multipoint command...')
- self.issue_multipoint__command()
+ self.issue_multipoint_command()
time.sleep(2)
if __name__ == '__main__':
try:
+ # Initialize the MultiPointCommand() class and set it to node and run the
+ # main() function.
node = MultiPointCommand()
node.main()
except KeyboardInterrupt: