@ -97,24 +92,23 @@ Every Python ROS [Node](http://wiki.ros.org/Nodes) will have this declaration at
```python
```python
import rclpy
import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from rclpy.duration import Duration
from rclpy.action import ActionClient
import sys
from control_msgs.action import FollowJointTrajectory
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from trajectory_msgs.msg import JointTrajectoryPoint
from sensor_msgs.msg import JointState
from hello_helpers.hello_misc import HelloNode
import time
```
```
You need to import rclpy if you are writing a ROS 2 Node. Import the FollowJointTrajectory from the [control_msgs.msg](http://wiki.ros.org/control_msgs) package to control the Stretch robot. Import JointTrajectoryPoint from the [trajectory_msgs](https://github.com/ros2/common_interfaces/tree/galactic/trajectory_msgs) package to define robot trajectories.
You need to import rclpy if you are writing a ROS 2 Node. Import the FollowJointTrajectory from the [control_msgs.action](https://github.com/ros-controls/control_msgs/tree/master/control_msgs) package to control the Stretch robot. Import JointTrajectoryPoint from the [trajectory_msgs](https://github.com/ros2/common_interfaces/tree/rolling/trajectory_msgs/msg) package to define robot trajectories.
The `StowCommand ` class inherits from the `Node` class from and is initialized.
The `StowCommand ` class inherits from the `HelloNode` class and is initialized with the main method in HelloNode by passing the arguments node_name as 'stow_command', node_namespace as 'stow_command' and wait_for_first_pointcloud as False. Refer to the [Introduction to HelloNode]() tutorial if you haven't already to understand how this works.
The `issue_stow_command()` method will stow Stretch's arm. Within the function, we set *stow_point* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. These are defined in the next set of the code.
The `issue_stow_command()` method will stow Stretch's arm. Within the function, we set *stow_point* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively. These are defined in the next set of the code.
```python
```python
stow_point1 = JointTrajectoryPoint()
stow_point2 = JointTrajectoryPoint()
duration1 = Duration(seconds=0.0)
duration2 = Duration(seconds=4.0)
stow_point1.time_from_start = duration1.to_msg()
stow_point2.time_from_start = duration2.to_msg()
joint_value1 = joint_state.position[1] # joint_lift is at index 1
joint_value2 = joint_state.position[0] # wrist_extension is at index 0
joint_value3 = joint_state.position[8] # joint_wrist_yaw is at index 8
Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by the positions set in *stow_point*. Specify the coordinate frame that we want (base_link) and set the time to be now.
Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by the positions set in *stow_point1* and *stow_point2*.
Create a funcion, `main()`, to do all of the setup in the class and issue the stow command. Initialize the `StowCommand()` class and set it to *node* and run the `main()` function.
Create a funcion, `main()`, to do all of the setup in the class and issue the stow command. Initialize the `StowCommand()` class and set it to *node* and run the `main()` function.
@ -181,7 +182,7 @@ To make the script executable call the main() function like above.
If you have killed the above instance of stretch_driver relaunch it again through the terminal.
If you have killed the above instance of stretch_driver relaunch it again through the terminal.
Seeing that there are similarities between the multipoint and stow command nodes, we will only breakdown the different components of the multipoint_command node.
Seeing that there are similarities between the multipoint and stow command nodes, we will only breakdown the distinct components of the multipoint_command node.
```python
```python
point1 = JointTrajectoryPoint()
point1 = JointTrajectoryPoint()
point1.positions = [0.3, 0.1, 2.0]
point1.positions = [0.9, 0.0, 0.0, 0.0]
point1.time_from_start = duration1.to_msg()
point1.time_from_start = duration1.to_msg()
```
```
Set *point1* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist extension, and yaw of the wrist, respectively.
Set *point1* as a `JointTrajectoryPoint`and provide desired positions (in meters). These are the positions of the lift, wrist_extension, wrist_yaw and gripper_aperture joints, respectively.
!!! note
!!! 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.
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.
Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` 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.
Set *trajectory_goal* as a `FollowJointTrajectory.Goal()` and define the joint names as a list. Then `trajectory_goal.trajectory.points` is defined by a list of the 6 points.
hello-chintan
1 year ago