From b9aebadac31a47c106543ea25d775d927e10bb2a Mon Sep 17 00:00:00 2001
From: "Alan G. Sanchez" <sanchala@oregonstate.edu>
Date: Fri, 26 Aug 2022 15:25:06 -0700
Subject: [PATCH] Updated python script and information about the
 wrist_extension.

---
 example_5.md | 29 ++++++++++++++++++-----------
 1 file changed, 18 insertions(+), 11 deletions(-)

diff --git a/example_5.md b/example_5.md
index 591501b..4a3e7b8 100644
--- a/example_5.md
+++ b/example_5.md
@@ -19,12 +19,12 @@ python joint_state_printer.py
 ```
 Your terminal will output the `position` information of the previously mentioned joints shown below.
 ```
-name: ['joint_lift', 'joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3', 'joint_wrist_yaw']
-position: [1.004518435897309, 0.12361581673760723, 0.023224914142933994, 0.07758496706423101, 0.12309362763409384, 1.8771004095879587]
+name: ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
+position: [0.6043133175850597, 0.19873586673129257, 0.017257283863713464]
 
 ```
 
-It's important to note that the arm has 4 prismatic joints and the sum of these positions gives the wrist extension distance. The wrist extension is needed when sending [joint trajectory commands](follow_joint_trajectory.md) to the robot. Here is an image of the arm joints for reference:
+**IMPORTANT NOTE:** Stretch's arm has 4 prismatic joints and the sum of these positions gives the *wrist_extension* distance. The *wrist_extension* is needed when sending [joint trajectory commands](follow_joint_trajectory.md) to the robot. Further, you **can not** actuate an individual arm joint. Here is an image of the arm joints for reference:
 
 <p align="center">
   <img src="images/joints.png"/>
@@ -64,7 +64,11 @@ class JointStatePublisher():
 		:param joints: A list of joint names.
 		"""
 		joint_positions = []
-		for joint in joints:
+    for joint in joints:
+			if joint == "wrist_extension":
+				index = self.joint_states.name.index('joint_arm_l0')
+				joint_positions.append(4*self.joint_states.position[index])
+				continue
 			index = self.joint_states.name.index(joint)
 			joint_positions.append(self.joint_states.position[index])
 		print("name: " + str(joints))
@@ -76,7 +80,7 @@ if __name__ == '__main__':
 	rospy.init_node('joint_state_printer', anonymous=True)
 	JSP = JointStatePublisher()
 	rospy.sleep(.1)
-	joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_13", "joint_wrist_yaw"]
+	joints = ["joint_lift", "wrist_extension", "joint_wrist_yaw"]
 	#joints = ["joint_head_pan","joint_head_tilt", joint_gripper_finger_left", "joint_gripper_finger_right"]
 	JSP.print_states(joints)
 	rospy.spin()
@@ -119,18 +123,21 @@ This is the `print_states()` function which takes in a list of joints of interes
 
 ```python
 for joint in joints:
-	index = self.joint_states.name.index(joint)
-	joint_positions.append(self.joint_states.position[index])
-print(joint_positions)
+  if joint == "wrist_extension":
+    index = self.joint_states.name.index('joint_arm_l0')
+    joint_positions.append(4*self.joint_states.position[index])
+    continue
+  index = self.joint_states.name.index(joint)
+  joint_positions.append(self.joint_states.position[index])
 ```
-In this section of the code, a forloop is used to parse the names of the requested joints from the *self.joint_states* list. The `index()` function returns the index a of the name of the requested joint and appends the respective position to our *joint_positions* list.
+In this section of the code, a for loop is used to parse the names of the requested joints from the *self.joint_states* list. The `index()` function returns the index a of the name of the requested joint and appends the respective position to our *joint_positions* list.
 
 ```python
 rospy.signal_shutdown("done")
 sys.exit(0)
 ```
 
-The first line of code initiates a clean shutodwn of ROS. The second line of code exits the Python interpreter.
+The first line of code initiates a clean shutdown of ROS. The second line of code exits the Python interpreter.
 
 ```python
 rospy.init_node('joint_state_printer', anonymous=True)
@@ -144,7 +151,7 @@ Declare object, *JSP*, from the `JointStatePublisher` class.
 The use of the `rospy.sleep()` function is to allow the *JSP* class to initialize all of its features before requesting to publish joint positions of desired joints (running the `print_states()` function).
 
 ```python
-joints = ["joint_lift", "joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_13", "joint_wrist_yaw"]
+joints = ["joint_lift", "wrist_extension", "joint_wrist_yaw"]
 #joints = ["joint_head_pan","joint_head_tilt", joint_gripper_finger_left", "joint_gripper_finger_right"]
 JSP.print_states(joints)
 ```