Skip to content

Commit

Permalink
2.0.0 (#3)
Browse files Browse the repository at this point in the history
Signed-off-by: Andrej Orsula <[email protected]>
AndrejOrsula authored Feb 3, 2022

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
2 parents 02478b4 + 67d9495 commit 70f8e00
Showing 7 changed files with 509 additions and 100 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -11,6 +11,7 @@ ament_python_install_package(pymoveit2)
# Install examples
set(EXAMPLES_DIR examples)
install(PROGRAMS
${EXAMPLES_DIR}/ex_gripper_command.py
${EXAMPLES_DIR}/ex_gripper.py
${EXAMPLES_DIR}/ex_joint_goal.py
${EXAMPLES_DIR}/ex_pose_goal.py
3 changes: 3 additions & 0 deletions examples/ex_gripper.py
Original file line number Diff line number Diff line change
@@ -48,6 +48,9 @@ def main(args=None):
executor_thread = Thread(target=executor.spin, daemon=True, args=())
executor_thread.start()

# Sleep a while in order to get the first joint state
node.create_rate(10.0).sleep()

# Get parameter
action = node.get_parameter("action").get_parameter_value().string_value

80 changes: 80 additions & 0 deletions examples/ex_gripper_command.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
#!/usr/bin/env python3
"""
Example of interacting with the gripper.
`ros2 run pymoveit2 ex_gripper_command.py --ros-args -p action:="toggle"`
`ros2 run pymoveit2 ex_gripper_command.py --ros-args -p action:="open"`
`ros2 run pymoveit2 ex_gripper_command.py --ros-args -p action:="close"`
"""

from threading import Thread

import rclpy
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node

from pymoveit2 import GripperCommand
from pymoveit2.robots import panda


def main(args=None):

rclpy.init(args=args)

# Create node for this example
node = Node("ex_gripper_command")

# Declare parameter for joint positions
node.declare_parameter(
"action",
"toggle",
)

# Create callback group that allows execution of callbacks in parallel without restrictions
callback_group = ReentrantCallbackGroup()

# Create MoveIt 2 gripper interface
moveit2_gripper = GripperCommand(
node=node,
gripper_joint_names=panda.gripper_joint_names(),
open_gripper_joint_positions=panda.OPEN_GRIPPER_JOINT_POSITIONS,
closed_gripper_joint_positions=panda.CLOSED_GRIPPER_JOINT_POSITIONS,
max_effort=10.0,
ignore_new_calls_while_executing=True,
callback_group=callback_group,
gripper_command_action_name="/robot_j2s7s300_gripper/gripper_command",
)

# Spin the node in background thread(s)
executor = rclpy.executors.MultiThreadedExecutor(2)
executor.add_node(node)
executor_thread = Thread(target=executor.spin, daemon=True, args=())
executor_thread.start()

# Sleep a while in order to get the first joint state
node.create_rate(10.0).sleep()

# Get parameter
action = node.get_parameter("action").get_parameter_value().string_value

# Perform gripper action
node.get_logger().info(f'Performing gripper action "{action}"')
if "open" == action:
moveit2_gripper.open()
moveit2_gripper.wait_until_executed()
elif "close" == action:
moveit2_gripper.close()
moveit2_gripper.wait_until_executed()
else:
period_s = 1.0
rate = node.create_rate(1 / period_s)
while rclpy.ok():
moveit2_gripper()
moveit2_gripper.wait_until_executed()
rate.sleep()

rclpy.shutdown()
exit(0)


if __name__ == "__main__":
main()
172 changes: 144 additions & 28 deletions pymoveit2/gripper_command.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
import threading
from typing import List, Optional, Union

from action_msgs.msg import GoalStatus
from control_msgs.action import GripperCommand
from control_msgs.action import GripperCommand as GripperCommandAction
from rclpy.action import ActionClient
from rclpy.callback_groups import CallbackGroup
from rclpy.node import Node
@@ -11,6 +12,7 @@
QoSProfile,
QoSReliabilityPolicy,
)
from sensor_msgs.msg import JointState


class GripperCommand:
@@ -21,17 +23,18 @@ class GripperCommand:
def __init__(
self,
node: Node,
gripper_joint_names: List[str],
open_gripper_joint_positions: Union[float, List[float]],
closed_gripper_joint_positions: Union[float, List[float]],
max_effort: float = 0.0,
ignore_new_calls_while_executing: bool = True,
callback_group: Optional[CallbackGroup] = None,
gripper_command_action_name: str = "gripper_action_controller/gripper_command",
**kwargs,
):
"""
Construct an instance of `MoveIt2Gripper` interface.
Construct an instance of `GripperCommand` interface.
- `node` - ROS 2 node that this interface is attached to
- `gripper_joint_names` - List of gripper joint names (can be extracted from URDF)
- `open_gripper_joint_positions` - Configuration of gripper joints when open
- `closed_gripper_joint_positions` - Configuration of gripper joints when fully closed
- `max_effort` - Max effort applied when closing
@@ -42,11 +45,26 @@ def __init__(
"""

self._node = node
self._callback_group = callback_group

# Create subscriber for current joint states
self._node.create_subscription(
msg_type=JointState,
topic="joint_states",
callback=self.__joint_state_callback,
qos_profile=QoSProfile(
durability=QoSDurabilityPolicy.VOLATILE,
reliability=QoSReliabilityPolicy.BEST_EFFORT,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1,
),
callback_group=self._callback_group,
)

# Create action client for move action
self.__gripper_command_action_client = ActionClient(
node=self._node,
action_type=GripperCommand,
action_type=GripperCommandAction,
action_name=gripper_command_action_name,
goal_service_qos_profile=QoSProfile(
durability=QoSDurabilityPolicy.VOLATILE,
@@ -78,26 +96,46 @@ def __init__(
history=QoSHistoryPolicy.KEEP_LAST,
depth=1,
),
callback_group=callback_group,
callback_group=self._callback_group,
)

# Initialise command goals for opening/closing
self.__open_gripper_joint_positions = open_gripper_joint_positions
self.__open_gripper_command_goal = self.__init_gripper_command_goal(
position=open_gripper_joint_positions, max_effort=max_effort
)
self.__close_gripper_command_goal = self.__init_gripper_command_goal(
position=closed_gripper_joint_positions, max_effort=max_effort
)

# Initialise internals for determining whether the gripper is open or closed
self.__joint_state_mutex = threading.Lock()
self.__joint_state = None
self.__new_joint_state_available = False
# Tolerance used for checking whether the gripper is open or closed
self.__open_tolerance = [
0.1
* abs(open_gripper_joint_positions[i] - closed_gripper_joint_positions[i])
for i in range(len(open_gripper_joint_positions))
]
# Indices of gripper joint within the joint state message topic.
# It is assumed that the order of these does not change during execution.
self.__gripper_joint_indices: Optional[List[int]] = None

# Flag that determines whether a new goal can be send while the previous one is being executed
self.__ignore_new_calls_while_executing = ignore_new_calls_while_executing
self.__is_executing = False

# Initialize additional variables
self.__is_open = True
# Store additional variables for later use
self.__joint_names = gripper_joint_names

# Internal states that monitor the current motion requests and execution
self.__is_motion_requested = False
self.__is_executing = False
self.__wait_until_executed_rate = self._node.create_rate(1000.0)

def __call__(self):
"""
Callable that is identical to `MoveIt2Gripper.toggle()`.
Callable that is identical to `GripperCommand.toggle()`.
"""

self.toggle()
@@ -108,9 +146,9 @@ def toggle(self):
"""

if self.is_open:
self.close()
self.close(skip_if_noop=False)
else:
self.open()
self.open(skip_if_noop=False)

def open(self, skip_if_noop: bool = True):
"""
@@ -123,6 +161,7 @@ def open(self, skip_if_noop: bool = True):

if self.__ignore_new_calls_while_executing and self.__is_executing:
return
self.__is_motion_requested = True

self.__send_goal_async_gripper_command(self.__open_gripper_command_goal)

@@ -137,6 +176,7 @@ def close(self, skip_if_noop: bool = True):

if self.__ignore_new_calls_while_executing and self.__is_executing:
return
self.__is_motion_requested = True

self.__send_goal_async_gripper_command(self.__close_gripper_command_goal)

@@ -146,27 +186,56 @@ def reset_open(self):
This is useful for simulated robots that allow instantaneous reset of joints.
"""

self.force_reset_executing_state()
self.__send_goal_async_gripper_command(self.__open_gripper_command_goal)

self.__is_open = True

def reset_closed(self):
"""
Reset into closed configuration by sending a dummy joint trajectory.
This is useful for simulated robots that allow instantaneous reset of joints.
"""

self.force_reset_executing_state()
self.__send_goal_async_gripper_command(self.__close_gripper_command_goal)

self.__is_open = False
def force_reset_executing_state(self):
"""
Force reset of internal states that block execution while `ignore_new_calls_while_executing` is being
used. This function is applicable only in a very few edge-cases, so it should almost never be used.
"""

self.__is_motion_requested = False
self.__is_executing = False

def wait_until_executed(self):
"""
Wait until the previously requested motion is finalised through either a success or failure.
"""

if not self.__is_motion_requested:
self._node.get_logger().warn(
"Cannot wait until motion is executed (no motion is in progress)."
)
return

while self.__is_motion_requested or self.__is_executing:
self.__wait_until_executed_rate.sleep()

def __joint_state_callback(self, msg: JointState):

def __toggle_internal_gripper_state(self):
# Update only if all relevant joints are included in the message
for joint_name in self.joint_names:
if not joint_name in msg.name:
return

self.__is_open = not self.__is_open
self.__joint_state_mutex.acquire()
self.__joint_state = msg
self.__new_joint_state_available = True
self.__joint_state_mutex.release()

def __send_goal_async_gripper_command(
self,
goal: GripperCommand.Goal,
goal: GripperCommandAction.Goal,
wait_for_server_timeout_sec: Optional[float] = 1.0,
):

@@ -190,12 +259,13 @@ def __response_callback_gripper_command(self, response):
goal_handle = response.result()
if not goal_handle.accepted:
self._node.get_logger().warn(
f"Action '{self.__gripper_command_action_client._action_name}' was rejected"
f"Action '{self.__gripper_command_action_client._action_name}' was rejected."
)
self.__is_motion_requested = False
return

if self.__ignore_new_calls_while_executing:
self.__is_executing = True
self.__is_executing = True
self.__is_motion_requested = False

self.__get_result_future_gripper_command = goal_handle.get_result_async()
self.__get_result_future_gripper_command.add_done_callback(
@@ -204,36 +274,82 @@ def __response_callback_gripper_command(self, response):

def __result_callback_gripper_command(self, res):

if res.result().status == GoalStatus.STATUS_SUCCEEDED:
self.__toggle_internal_gripper_state()
else:
if res.result().status != GoalStatus.STATUS_SUCCEEDED:
self._node.get_logger().error(
f"Action '{self.__gripper_command_action_client._action_name}' was unsuccessful: {res.result().status}"
)

if self.__ignore_new_calls_while_executing:
self.__is_executing = False
self.__is_executing = False

@classmethod
def __init_gripper_command_goal(
cls, position: Union[float, List[float]], max_effort: float
) -> GripperCommand.Goal:
) -> GripperCommandAction.Goal:

if hasattr(position, "__getitem__"):
position = position[0]

gripper_cmd_goal = GripperCommand.Goal()
gripper_cmd_goal = GripperCommandAction.Goal()
gripper_cmd_goal.command.position = position
gripper_cmd_goal.command.max_effort = max_effort

return gripper_cmd_goal

@property
def joint_names(self) -> List[str]:

return self.__joint_names

@property
def joint_state(self) -> Optional[JointState]:

self.__joint_state_mutex.acquire()
joint_state = self.__joint_state
self.__joint_state_mutex.release()
return joint_state

@property
def new_joint_state_available(self):

return self.__new_joint_state_available

@property
def is_open(self) -> bool:
"""
Gripper is considered to be open if all of the joints are at their open position.
"""

joint_state = self.joint_state

# Assume the gripper is open if there are no joint state readings yet
if joint_state is None:
return True

return self.__is_open
# For the sake of performance, find the indices of joints only once.
# This is especially useful for robots with many joints.
if self.__gripper_joint_indices is None:
self.__gripper_joint_indices: List[int] = []
for joint_name in self.joint_names:
self.__gripper_joint_indices.append(joint_state.name.index(joint_name))

for local_joint_index, joint_state_index in enumerate(
self.__gripper_joint_indices
):
if (
abs(
joint_state.position[joint_state_index]
- self.__open_gripper_joint_positions[local_joint_index]
)
> self.__open_tolerance[local_joint_index]
):
return False

return True

@property
def is_closed(self) -> bool:
"""
Gripper is considered to be closed if any of the joints is outside of their open position.
"""

return not self.is_open
202 changes: 152 additions & 50 deletions pymoveit2/moveit2.py

Large diffs are not rendered by default.

106 changes: 91 additions & 15 deletions pymoveit2/moveit2_gripper.py
Original file line number Diff line number Diff line change
@@ -1,9 +1,10 @@
import math
from typing import List, Optional

from rclpy.callback_groups import CallbackGroup
from rclpy.node import Node

from .moveit2 import MoveIt2
from .moveit2 import *


class MoveIt2Gripper(MoveIt2):
@@ -21,6 +22,8 @@ def __init__(
gripper_group_name: str = "gripper",
execute_via_moveit: bool = False,
ignore_new_calls_while_executing: bool = False,
skip_planning: bool = False,
skip_planning_fixed_motion_duration: float = 0.5,
callback_group: Optional[CallbackGroup] = None,
follow_joint_trajectory_action_name: str = "gripper_trajectory_controller/follow_joint_trajectory",
):
@@ -36,6 +39,11 @@ def __init__(
together with a separate planning service client
- `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories
while previous is still being executed
- `skip_planning` - If enabled, planning is skipped and a single joint trajectory point is published
for closing or opening. This enables much faster operation, but the collision
checking is disabled and the motion smoothness will depend on the controller.
- `skip_planning_fixed_motion_duration` - Desired duration for the closing and opening motions when
`skip_planning` mode is enabled.
- `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions)
- `follow_joint_trajectory_action_name` - Name of the action server for the controller
"""
@@ -61,12 +69,42 @@ def __init__(
self.__open_gripper_joint_positions = open_gripper_joint_positions
self.__closed_gripper_joint_positions = closed_gripper_joint_positions

self.__skip_planning = skip_planning
if skip_planning:
duration_sec = math.floor(skip_planning_fixed_motion_duration)
duration_nanosec = int(
10e8 * (skip_planning_fixed_motion_duration - duration_sec)
)
self.__open_dummy_trajectory_goal = init_follow_joint_trajectory_goal(
init_dummy_joint_trajectory_from_state(
init_joint_state(
joint_names=gripper_joint_names,
joint_positions=open_gripper_joint_positions,
),
duration_sec=duration_sec,
duration_nanosec=duration_nanosec,
)
)
self.__close_dummy_trajectory_goal = init_follow_joint_trajectory_goal(
init_dummy_joint_trajectory_from_state(
init_joint_state(
joint_names=gripper_joint_names,
joint_positions=closed_gripper_joint_positions,
),
duration_sec=duration_sec,
duration_nanosec=duration_nanosec,
)
)

# Tolerance used for checking whether the gripper is open or closed
self.__open_tolerance = [
0.1
* abs(open_gripper_joint_positions[i] - closed_gripper_joint_positions[i])
for i in range(len(gripper_joint_names))
]
# Indices of gripper joint within the joint state message topic.
# It is assumed that the order of these does not change during execution.
self.__gripper_joint_indices: Optional[List[int]] = None

def __call__(self):
"""
@@ -94,7 +132,12 @@ def open(self, skip_if_noop: bool = True):
if skip_if_noop and self.is_open:
return

self.move_to_configuration(joint_positions=self.__open_gripper_joint_positions)
if self.__skip_planning:
self.__open_without_planning()
else:
self.move_to_configuration(
joint_positions=self.__open_gripper_joint_positions
)

def close(self, skip_if_noop: bool = True):
"""
@@ -105,25 +148,46 @@ def close(self, skip_if_noop: bool = True):
if skip_if_noop and self.is_closed:
return

self.move_to_configuration(
joint_positions=self.__closed_gripper_joint_positions
)
if self.__skip_planning:
self.__close_without_planning()
else:
self.move_to_configuration(
joint_positions=self.__closed_gripper_joint_positions
)

def reset_open(self):
def reset_open(self, sync: bool = True):
"""
Reset into open configuration by sending a dummy joint trajectory.
This is useful for simulated robots that allow instantaneous reset of joints.
"""

self.reset_controller(joint_state=self.__open_gripper_joint_positions)
self.reset_controller(
joint_state=self.__open_gripper_joint_positions, sync=sync
)

def reset_closed(self):
def reset_closed(self, sync: bool = True):
"""
Reset into closed configuration by sending a dummy joint trajectory.
This is useful for simulated robots that allow instantaneous reset of joints.
"""

self.reset_controller(joint_state=self.__closed_gripper_joint_positions)
self.reset_controller(
joint_state=self.__closed_gripper_joint_positions, sync=sync
)

def __open_without_planning(self):

self._send_goal_async_follow_joint_trajectory(
goal=self.__open_dummy_trajectory_goal,
wait_until_response=False,
)

def __close_without_planning(self):

self._send_goal_async_follow_joint_trajectory(
goal=self.__close_dummy_trajectory_goal,
wait_until_response=False,
)

def __del_redundant_attributes(self):

@@ -140,16 +204,28 @@ def is_open(self) -> bool:
Gripper is considered to be open if all of the joints are at their open position.
"""

for i in range(len(self.joint_names)):
joint_state = self.joint_state

# Assume the gripper is open if there are no joint state readings yet
if joint_state is None:
return True

# For the sake of performance, find the indices of joints only once.
# This is especially useful for robots with many joints.
if self.__gripper_joint_indices is None:
self.__gripper_joint_indices: List[int] = []
for joint_name in self.joint_names:
self.__gripper_joint_indices.append(joint_state.name.index(joint_name))

for local_joint_index, joint_state_index in enumerate(
self.__gripper_joint_indices
):
if (
abs(
self.joint_state.position[
self.joint_state.name.index(self.joint_names[i])
]
- self.__open_gripper_joint_positions[i]
joint_state.position[joint_state_index]
- self.__open_gripper_joint_positions[local_joint_index]
)
> self.__open_tolerance[i]
> self.__open_tolerance[local_joint_index]
):
return False

45 changes: 38 additions & 7 deletions pymoveit2/moveit2_servo.py
Original file line number Diff line number Diff line change
@@ -123,7 +123,10 @@ def servo(
self._node.get_logger().warn(
f"Calling '{self.__start_service.srv_name}' service to enable MoveIt Servo..."
)
self.enable()
if not self.enable():
return
else:
return

twist_msg = deepcopy(self.__twist_msg)
twist_msg.header.stamp = self._node.get_clock().now().to_msg()
@@ -135,7 +138,9 @@ def servo(
twist_msg.twist.angular.z *= angular[2]
self.__twist_pub.publish(twist_msg)

def enable(self, wait_for_server_timeout_sec: Optional[float] = 1.0):
def enable(
self, wait_for_server_timeout_sec: Optional[float] = 1.0, sync: bool = False
) -> bool:
"""
Enable MoveIt 2 Servo server via async service call.
"""
@@ -146,10 +151,24 @@ def enable(self, wait_for_server_timeout_sec: Optional[float] = 1.0):
self._node.get_logger().warn(
f"Service '{self.__start_service.srv_name}' is not yet available..."
)
start_service_future = self.__start_service.call_async(self.__trigger_req)
start_service_future.add_done_callback(self.__enable_done_callback)
return False

def disable(self, wait_for_server_timeout_sec: Optional[float] = 1.0):
if sync:
result = self.__start_service.call(self.__trigger_req)
if not result.success:
self._node.get_logger().error(
f"MoveIt Servo could not be enabled. ({result.message})"
)
self.__is_enabled = result.success
return result.success
else:
start_service_future = self.__start_service.call_async(self.__trigger_req)
start_service_future.add_done_callback(self.__enable_done_callback)
return True

def disable(
self, wait_for_server_timeout_sec: Optional[float] = 1.0, sync: bool = False
) -> bool:
"""
Disable MoveIt 2 Servo server via async service call.
"""
@@ -160,8 +179,20 @@ def disable(self, wait_for_server_timeout_sec: Optional[float] = 1.0):
self._node.get_logger().warn(
f"Service '{self.__stop_service.srv_name}' is not yet available..."
)
stop_service_future = self.__stop_service.call_async(self.__trigger_req)
stop_service_future.add_done_callback(self.__disable_done_callback)
return False

if sync:
result = self.__stop_service.call(self.__trigger_req)
if not result.success:
self._node.get_logger().error(
f"MoveIt Servo could not be disabled. ({result.message})"
)
self.__is_enabled = not result.success
return result.success
else:
stop_service_future = self.__stop_service.call_async(self.__trigger_req)
stop_service_future.add_done_callback(self.__disable_done_callback)
return True

def __enable_done_callback(self, future: Future):

0 comments on commit 70f8e00

Please sign in to comment.