ROS & ROS2: Command the robot
Stop the robot
Stop the current motion
Default topic name |
Message type (msg) |
---|---|
|
Example
$ rostopic pub -1 /iiwa/command/stop std_msgs/Empty "{}"
import rospy
import std_msgs.msg
# create a publisher
pub = rospy.Publisher('/iiwa/command/stop', std_msgs.msg.Empty, queue_size=1)
# initialize the ROS node
rospy.init_node('test')
# publish the message
pub.publish(std_msgs.msg.Empty())
$ ros2 topic pub -1 /iiwa/command/stop std_msgs/msg/Empty "{}"
import rclpy
import std_msgs.msg
from rclpy.node import Node
from rclpy.qos import QoSPresetProfiles
# initialize the ROS node
rclpy.init()
node = Node('test')
# create a publisher
pub = node.create_publisher(std_msgs.msg.Empty, '/iiwa/command/stop', QoSPresetProfiles.SYSTEM_DEFAULT.value)
# publish the message
pub.publish(std_msgs.msg.Empty())
try:
rclpy.spin_once(node)
except KeyboardInterrupt:
pass
finally:
# shutdown the node
node.destroy_node()
rclpy.shutdown()
Joint space
Move the robot to the specified joint position
Default topic name |
Message type (msg) |
Units |
---|---|---|
|
\(radians\) |
Implementation details
Joint names are the same as those of the iiwa_description URDF model (
iiwa_joint_1
,iiwa_joint_2
,iiwa_joint_3
,iiwa_joint_4
,iiwa_joint_5
,iiwa_joint_6
,iiwa_joint_7
)Joint names, and their respective positions, can appear in any order
No need to specify message name if all joint positions are specified
A subset of the joints can be controlled by specifying only the joint names and the corresponding positions, or by setting the other joints positions to
nan
. In this case, the excluded joints will keep their current position
Example
Move all joints to their respective goal positions
Joint names: [
iiwa_joint_1
,iiwa_joint_2
,iiwa_joint_3
,iiwa_joint_4
,iiwa_joint_5
,iiwa_joint_6
,iiwa_joint_7
]Joint positions: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
# name are specified
$ rostopic pub -1 /iiwa/command/joint sensor_msgs/JointState "{name: ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'], position: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]}"
# no need to specify message name if all joint positions are specified
$ rostopic pub -1 /iiwa/command/joint sensor_msgs/JointState "{position: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]}"
import rospy
import sensor_msgs.msg
# create a publisher
pub = rospy.Publisher('/iiwa/command/joint', sensor_msgs.msg.JointState, queue_size=1)
# initialize the ROS node
rospy.init_node('test')
# build a message
msg = sensor_msgs.msg.JointState()
msg.position = [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
# or
msg = sensor_msgs.msg.JointState()
msg.name = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7']
msg.position = [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
# publish the message
pub.publish(msg)
# name are specified
$ ros2 topic pub -1 /iiwa/command/joint sensor_msgs/msg/JointState "{name: ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'], position: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]}"
# no need to specify message name if all joint positions are specified
$ ros2 topic pub -1 /iiwa/command/joint sensor_msgs/msg/JointState "{position: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]}"
import rclpy
import sensor_msgs.msg
from rclpy.node import Node
from rclpy.qos import QoSPresetProfiles
# initialize the ROS node
rclpy.init()
node = Node('test')
# create a publisher
pub = node.create_publisher(sensor_msgs.msg.JointState, '/iiwa/command/joint', QoSPresetProfiles.SYSTEM_DEFAULT.value)
# build a message
msg = sensor_msgs.msg.JointState()
msg.position = [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
# or
msg = sensor_msgs.msg.JointState()
msg.name = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3',
'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7']
msg.position = [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
# publish the message
pub.publish(msg)
try:
rclpy.spin_once(node)
except KeyboardInterrupt:
pass
finally:
# shutdown the node
node.destroy_node()
rclpy.shutdown()
Move only the specified joints to their respective goal positions
Joint names: [
iiwa_joint_4
,iiwa_joint_6
]Joint positions: [-1.57, 1.57]
Other joints will keep their current position
# all names and positions are specified
$ rostopic pub -1 /iiwa/command/joint sensor_msgs/JointState "{name: ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'], position: [.nan, .nan, .nan, -1.57, .nan, 1.57, .nan]}"
# all positions are specified
$ rostopic pub -1 /iiwa/command/joint sensor_msgs/JointState "{position: [.nan, .nan, .nan, -1.57, .nan, 1.57, .nan]}"
# only target joint names and positions are specified
$ rostopic pub -1 /iiwa/command/joint sensor_msgs/JointState "{name: ['iiwa_joint_4', 'iiwa_joint_6'], position: [-1.57, 1.57]}"
import math
import rospy
import sensor_msgs.msg
# create a publisher
pub = rospy.Publisher('/iiwa/command/joint', sensor_msgs.msg.JointState, queue_size=1)
# initialize the ROS node
rospy.init_node('test')
# build a message
# math.nan, float("nan"), np.nan and np.NaN are all equivalent
msg = sensor_msgs.msg.JointState()
msg.position = [math.nan, math.nan, math.nan, -1.57, math.nan, 1.57, math.nan]
# or
msg = sensor_msgs.msg.JointState()
msg.name = ['iiwa_joint_4', 'iiwa_joint_6']
msg.position = [-1.57, 1.57]
# publish the message
pub.publish(msg)
# all names and positions are specified
$ ros2 topic pub -1 /iiwa/command/joint sensor_msgs/msg/JointState "{name: ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'], position: [.nan, .nan, .nan, -1.57, .nan, 1.57, .nan]}"
# all positions are specified
$ ros2 topic pub -1 /iiwa/command/joint sensor_msgs/msg/JointState "{position: [.nan, .nan, .nan, -1.57, .nan, 1.57, .nan]}"
# only target joint names and positions are specified
$ ros2 topic pub -1 /iiwa/command/joint sensor_msgs/msg/JointState "{name: ['iiwa_joint_4', 'iiwa_joint_6'], position: [-1.57, 1.57]}"
import math
import rclpy
import sensor_msgs.msg
from rclpy.node import Node
from rclpy.qos import QoSPresetProfiles
# initialize the ROS node
rclpy.init()
node = Node('test')
# create a publisher
pub = node.create_publisher(sensor_msgs.msg.JointState, '/iiwa/command/joint', QoSPresetProfiles.SYSTEM_DEFAULT.value)
# build a message
# math.nan, float("nan"), np.nan and np.NaN are all equivalent
msg = sensor_msgs.msg.JointState()
msg.position = [math.nan, math.nan, math.nan, -1.57, math.nan, 1.57, math.nan]
# or
msg = sensor_msgs.msg.JointState()
msg.name = ['iiwa_joint_4', 'iiwa_joint_6']
msg.position = [-1.57, 1.57]
# publish the message
pub.publish(msg)
try:
rclpy.spin_once(node)
except KeyboardInterrupt:
pass
finally:
# shutdown the node
node.destroy_node()
rclpy.shutdown()
Cartesian space
Move the robot to the specified Cartesian pose
Default topic name |
Message type (msg) |
Units |
---|---|---|
|
position (\(m\)), orientation as quaternion |
Implementation details
Values set to
nan
will keep their current position/orientation
Example
Move to the specified Cartesian pose (position and orientation)
Cartesian position: X, Y, Z = (0.65, 0.0, 0.2)
Cartesian orientation: x, y, z, w = (0.0, 1.0, 0.0, 0.0) \(\; \rightarrow \;\) A, B, C = (-180.0º, 0.0º, 180.0º)
$ rostopic pub -1 /iiwa/command/cartesian geometry_msgs/Pose "{position: {x: 0.65, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 1.0, z: 0.0, w: 0.0}}"
import rospy
import geometry_msgs.msg
# create a publisher
pub = rospy.Publisher('/iiwa/command/cartesian', geometry_msgs.msg.Pose, queue_size=1)
# initialize the ROS node
rospy.init_node('test')
# build a message
msg = geometry_msgs.msg.Pose()
msg.position.x = 0.65
msg.position.y = 0.0
msg.position.z = 0.2
msg.orientation.x = 0.0
msg.orientation.y = 1.0
msg.orientation.z = 0.0
msg.orientation.w = 0.0
# publish the message
pub.publish(msg)
$ ros2 topic pub -1 /iiwa/command/cartesian geometry_msgs/msg/Pose "{position: {x: 0.65, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 1.0, z: 0.0, w: 0.0}}"
import rclpy
import geometry_msgs.msg
from rclpy.node import Node
from rclpy.qos import QoSPresetProfiles
# initialize the ROS node
rclpy.init()
node = Node('test')
# create a publisher
pub = node.create_publisher(geometry_msgs.msg.Pose, '/iiwa/command/cartesian', QoSPresetProfiles.SYSTEM_DEFAULT.value)
# build a message
msg = geometry_msgs.msg.Pose()
msg.position.x = 0.65
msg.position.y = 0.0
msg.position.z = 0.2
msg.orientation.x = 0.0
msg.orientation.y = 1.0
msg.orientation.z = 0.0
msg.orientation.w = 0.0
# publish the message
pub.publish(msg)
try:
rclpy.spin_once(node)
except KeyboardInterrupt:
pass
finally:
# shutdown the node
node.destroy_node()
rclpy.shutdown()
Move to the specified Cartesian position or orientation
- Case 1
Cartesian position: X, Y, Z = (0.65, 0.0, 0.3)
Keep the current orientation
- Case 2
Move in Z-axis only: Z = 0.4
Keep the current position in X, Y and orientation
- Case 3
Cartesian orientation: x, y, z, w = (0.0, -0.7071, 0.7071, 0.0) \(\; \rightarrow \;\) A, B, C = (90.0º, 0.0º, 180.0º)
Keep the current position
# case 1
$ rostopic pub -1 /iiwa/command/cartesian geometry_msgs/Pose "{position: {x: 0.65, y: 0.0, z: 0.3}, orientation: {x: .nan, y: .nan, z: .nan, w: .nan}}"
# case 2
$ rostopic pub -1 /iiwa/command/cartesian geometry_msgs/Pose "{position: {x: .nan, y: .nan, z: 0.4}, orientation: {x: .nan, y: .nan, z: .nan, w: .nan}}"
# case 3
$ rostopic pub -1 /iiwa/command/cartesian geometry_msgs/Pose "{position: {x: .nan, y: .nan, z: .nan}, orientation: {x: 0.0, y: -0.7071, z: 0.7071, w: 0.0}}"
import math
import rospy
import geometry_msgs.msg
# create a publisher
pub = rospy.Publisher('/iiwa/command/cartesian', geometry_msgs.msg.Pose, queue_size=1)
# initialize the ROS node
rospy.init_node('test')
# build a message
# math.nan, float("nan"), np.nan and np.NaN are all equivalent
# case 1
msg = geometry_msgs.msg.Pose()
msg.position.x = 0.65
msg.position.y = 0.0
msg.position.z = 0.3
msg.orientation.x = math.nan
msg.orientation.y = math.nan
msg.orientation.z = math.nan
msg.orientation.w = math.nan
# case 2
msg = geometry_msgs.msg.Pose()
msg.position.x = math.nan
msg.position.y = math.nan
msg.position.z = 0.4
msg.orientation.x = math.nan
msg.orientation.y = math.nan
msg.orientation.z = math.nan
msg.orientation.w = math.nan
# case 3
msg = geometry_msgs.msg.Pose()
msg.position.x = math.nan
msg.position.y = math.nan
msg.position.z = math.nan
msg.orientation.x = 0.0
msg.orientation.y = -0.7071
msg.orientation.z = 0.7071
msg.orientation.w = 0.0
# publish the message
pub.publish(msg)
# case 1
$ ros2 topic pub -1 /iiwa/command/cartesian geometry_msgs/msg/Pose "{position: {x: 0.65, y: 0.0, z: 0.3}, orientation: {x: .nan, y: .nan, z: .nan, w: .nan}}"
# case 2
$ ros2 topic pub -1 /iiwa/command/cartesian geometry_msgs/msg/Pose "{position: {x: .nan, y: .nan, z: 0.4}, orientation: {x: .nan, y: .nan, z: .nan, w: .nan}}"
# case 3
$ ros2 topic pub -1 /iiwa/command/cartesian geometry_msgs/msg/Pose "{position: {x: .nan, y: .nan, z: .nan}, orientation: {x: 0.0, y: -0.7071, z: 0.7071, w: 0.0}}"
import math
import rclpy
import geometry_msgs.msg
from rclpy.node import Node
from rclpy.qos import QoSPresetProfiles
# initialize the ROS node
rclpy.init()
node = Node('test')
# create a publisher
pub = node.create_publisher(geometry_msgs.msg.Pose, '/iiwa/command/cartesian', QoSPresetProfiles.SYSTEM_DEFAULT.value)
# build a message
# math.nan, float("nan"), np.nan and np.NaN are all equivalent
# case 1
msg = geometry_msgs.msg.Pose()
msg.position.x = 0.65
msg.position.y = 0.0
msg.position.z = 0.3
msg.orientation.x = math.nan
msg.orientation.y = math.nan
msg.orientation.z = math.nan
msg.orientation.w = math.nan
# case 2
msg = geometry_msgs.msg.Pose()
msg.position.x = math.nan
msg.position.y = math.nan
msg.position.z = 0.4
msg.orientation.x = math.nan
msg.orientation.y = math.nan
msg.orientation.z = math.nan
msg.orientation.w = math.nan
# case 3
msg = geometry_msgs.msg.Pose()
msg.position.x = math.nan
msg.position.y = math.nan
msg.position.z = math.nan
msg.orientation.x = 0.0
msg.orientation.y = -0.7071
msg.orientation.z = 0.7071
msg.orientation.w = 0.0
# publish the message
pub.publish(msg)
try:
rclpy.spin_once(node)
except KeyboardInterrupt:
pass
finally:
# shutdown the node
node.destroy_node()
rclpy.shutdown()
Circular motion
# TODO