Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

3.2.0 #41

Merged
merged 4 commits into from
Mar 6, 2024
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Added Async Forward/Inverse Kinematics (#43)
* [WIP] Add async service call for FK/IK analogous to planning

* Compute FK returns a list of post_stampeds

* Pre-commit formatting

* Added FK example

* Comment changes to orientation path constraint examples

* Added IK example

* Update examples/ex_fk.py

Co-authored-by: Andrej Orsula <[email protected]>

* Update examples/ex_ik.py

Co-authored-by: Andrej Orsula <[email protected]>

* Update moveit2.py

---------

Co-authored-by: Ethan Gordon <[email protected]>
Co-authored-by: Andrej Orsula <[email protected]>
  • Loading branch information
3 people authored Feb 20, 2024
commit 09ce8d8a75afaf74da907b07a3a63e53cde70393
2 changes: 2 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -13,7 +13,9 @@ set(EXAMPLES_DIR examples)
install(PROGRAMS
${EXAMPLES_DIR}/ex_collision_mesh.py
${EXAMPLES_DIR}/ex_collision_primitive.py
${EXAMPLES_DIR}/ex_fk.py
${EXAMPLES_DIR}/ex_gripper.py
${EXAMPLES_DIR}/ex_ik.py
${EXAMPLES_DIR}/ex_joint_goal.py
${EXAMPLES_DIR}/ex_orientation_path_constraint.py
${EXAMPLES_DIR}/ex_pose_goal.py
90 changes: 90 additions & 0 deletions examples/ex_fk.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,90 @@
#!/usr/bin/env python3
"""
Example of computing Forward Kinematics.
- ros2 run pymoveit2 ex_fk.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]"
- ros2 run pymoveit2 ex_fk.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -p synchronous:=False
"""

from threading import Thread

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

from pymoveit2 import MoveIt2, MoveIt2State
from pymoveit2.robots import panda


def main():
rclpy.init()

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

# Declare parameter for joint positions
node.declare_parameter(
"joint_positions",
[
0.0,
0.0,
0.0,
-0.7853981633974483,
0.0,
1.5707963267948966,
0.7853981633974483,
],
)
node.declare_parameter("synchronous", True)

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

# Create MoveIt 2 interface
moveit2 = MoveIt2(
node=node,
joint_names=panda.joint_names(),
base_link_name=panda.base_link_name(),
end_effector_name=panda.end_effector_name(),
group_name=panda.MOVE_GROUP_ARM,
callback_group=callback_group,
)

# Spin the node in background thread(s) and wait a bit for initialization
executor = rclpy.executors.MultiThreadedExecutor(2)
executor.add_node(node)
executor_thread = Thread(target=executor.spin, daemon=True, args=())
executor_thread.start()
node.create_rate(1.0).sleep()

# Get parameters
joint_positions = (
node.get_parameter("joint_positions").get_parameter_value().double_array_value
)
synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value

# Move to joint configuration
node.get_logger().info(
f"Computing FK for {{joint_positions: {list(joint_positions)}}}"
)
retval = None
if synchronous:
retval = moveit2.compute_fk(joint_positions)
else:
future = moveit2.compute_fk_async(joint_positions)
if future is not None:
rate = node.create_rate(10)
while not future.done():
rate.sleep()
retval = moveit2.get_compute_fk_result(future)
if retval is None:
print("Failed.")
else:
print("Succeeded. Result: " + str(retval))

rclpy.shutdown()
executor_thread.join()
exit(0)


if __name__ == "__main__":
main()
79 changes: 79 additions & 0 deletions examples/ex_ik.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
#!/usr/bin/env python3
"""
Example of computing Inverse Kinematics.
- ros2 run pymoveit2 ex_ik.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]"
- ros2 run pymoveit2 ex_ik.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p synchronous:=False
"""

from threading import Thread

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

from pymoveit2 import MoveIt2, MoveIt2State
from pymoveit2.robots import panda


def main():
rclpy.init()

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

# Declare parameters for position and orientation
node.declare_parameter("position", [0.5, 0.0, 0.25])
node.declare_parameter("quat_xyzw", [1.0, 0.0, 0.0, 0.0])
node.declare_parameter("synchronous", True)

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

# Create MoveIt 2 interface
moveit2 = MoveIt2(
node=node,
joint_names=panda.joint_names(),
base_link_name=panda.base_link_name(),
end_effector_name=panda.end_effector_name(),
group_name=panda.MOVE_GROUP_ARM,
callback_group=callback_group,
)

# Spin the node in background thread(s) and wait a bit for initialization
executor = rclpy.executors.MultiThreadedExecutor(2)
executor.add_node(node)
executor_thread = Thread(target=executor.spin, daemon=True, args=())
executor_thread.start()
node.create_rate(1.0).sleep()

# Get parameters
position = node.get_parameter("position").get_parameter_value().double_array_value
quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value

# Move to joint configuration
node.get_logger().info(
f"Computing IK for {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
)
retval = None
if synchronous:
retval = moveit2.compute_ik(position, quat_xyzw)
else:
future = moveit2.compute_ik_async(position, quat_xyzw)
if future is not None:
rate = node.create_rate(10)
while not future.done():
rate.sleep()
retval = moveit2.get_compute_ik_result(future)
if retval is None:
print("Failed.")
else:
print("Succeeded. Result: " + str(retval))

rclpy.shutdown()
executor_thread.join()
exit(0)


if __name__ == "__main__":
main()
4 changes: 2 additions & 2 deletions examples/ex_orientation_path_constraint.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#!/usr/bin/env python3
"""
Example of moving to a joint configuration.
Example of moving to a joint configuration with orientation path constraints.
- ros2 run pymoveit2 ex_orientation_path_constraints.py --ros-args -p use_orientation_constraint:=True
- ros2 run pymoveit2 ex_orientation_path_constraints.py --ros-args -p use_orientation_constraint:=False
"""
@@ -19,7 +19,7 @@ def main():
rclpy.init()

# Create node for this example
node = Node("ex_joint_goal")
node = Node("ex_orientation_path_constraints")

# Declare parameter for joint positions
node.declare_parameter(
124 changes: 100 additions & 24 deletions pymoveit2/moveit2.py
Original file line number Diff line number Diff line change
@@ -479,7 +479,7 @@ def plan(
if future is None:
return None

# 10ms sleep
# 100ms sleep
rate = self._node.create_rate(10)
while not future.done():
rate.sleep()
@@ -1120,8 +1120,57 @@ def compute_fk(
self,
joint_state: Optional[Union[JointState, List[float]]] = None,
fk_link_names: Optional[List[str]] = None,
wait_for_server_timeout_sec: Optional[float] = 1.0,
) -> Optional[PoseStamped]:
) -> Optional[Union[PoseStamped, List[PoseStamped]]]:
"""
Call compute_fk_async and wait on future
"""
future = self.compute_fk_async(
**{key: value for key, value in locals().items() if key != "self"}
)

if future is None:
return None

# 100ms sleep
rate = self._node.create_rate(10)
while not future.done():
rate.sleep()

return self.get_compute_fk_result(future, fk_link_names=fk_link_names)

def get_compute_fk_result(
self,
future: Future,
fk_link_names: Optional[List[str]] = None,
) -> Optional[Union[PoseStamped, List[PoseStamped]]]:
"""
Takes in a future returned by compute_fk_async and returns the poses
if the future is done and successful, else None.
"""
if not future.done():
self._node.get_logger().warn(
"Cannot get FK result because future is not done."
)
return None

res = future.result()

if MoveItErrorCodes.SUCCESS == res.error_code.val:
if fk_link_names is None:
return res.pose_stamped[0]
else:
return res.pose_stamped
else:
self._node.get_logger().warn(
f"FK computation failed! Error code: {res.error_code.val}."
)
return None

def compute_fk_async(
self,
joint_state: Optional[Union[JointState, List[float]]] = None,
fk_link_names: Optional[List[str]] = None,
) -> Optional[Future]:
"""
Compute forward kinematics for all `fk_link_names` in a given `joint_state`.
- `fk_link_names` defaults to end-effector
@@ -1150,36 +1199,71 @@ def compute_fk(
stamp = self._node.get_clock().now().to_msg()
self.__compute_fk_req.header.stamp = stamp

if not self.__compute_fk_client.wait_for_service(
timeout_sec=wait_for_server_timeout_sec
):
if not self.__compute_fk_client.service_is_ready():
self._node.get_logger().warn(
f"Service '{self.__compute_fk_client.srv_name}' is not yet available. Better luck next time!"
)
return None

res = self.__compute_fk_client.call(self.__compute_fk_req)
return self.__compute_fk_client.call_async(self.__compute_fk_req)

def compute_ik(
self,
position: Union[Point, Tuple[float, float, float]],
quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]],
start_joint_state: Optional[Union[JointState, List[float]]] = None,
constraints: Optional[Constraints] = None,
wait_for_server_timeout_sec: Optional[float] = 1.0,
) -> Optional[JointState]:
"""
Call compute_ik_async and wait on future
"""
future = self.compute_ik_async(
**{key: value for key, value in locals().items() if key != "self"}
)

if future is None:
return None

# 10ms sleep
rate = self._node.create_rate(10)
while not future.done():
rate.sleep()

return self.get_compute_ik_result(future)

def get_compute_ik_result(
self,
future: Future,
) -> Optional[JointState]:
"""
Takes in a future returned by compute_ik_async and returns the joint states
if the future is done and successful, else None.
"""
if not future.done():
self._node.get_logger().warn(
"Cannot get IK result because future is not done."
)
return None

res = future.result()

if MoveItErrorCodes.SUCCESS == res.error_code.val:
pose_stamped = res.pose_stamped
if isinstance(pose_stamped, List):
return res.pose_stamped[0]
else:
return res.pose_stamped
return res.solution.joint_state
else:
self._node.get_logger().warn(
f"FK computation failed! Error code: {res.error_code.val}."
f"IK computation failed! Error code: {res.error_code.val}."
)
return None

def compute_ik(
def compute_ik_async(
self,
position: Union[Point, Tuple[float, float, float]],
quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]],
start_joint_state: Optional[Union[JointState, List[float]]] = None,
constraints: Optional[Constraints] = None,
wait_for_server_timeout_sec: Optional[float] = 1.0,
) -> Optional[JointState]:
) -> Optional[Future]:
"""
Compute inverse kinematics for the given pose. To indicate beginning of the search space,
`start_joint_state` can be specified. Furthermore, `constraints` can be imposed on the
@@ -1248,15 +1332,7 @@ def compute_ik(
)
return None

res = self.__compute_ik_client.call(self.__compute_ik_req)

if MoveItErrorCodes.SUCCESS == res.error_code.val:
return res.solution.joint_state
else:
self._node.get_logger().warn(
f"IK computation failed! Error code: {res.error_code.val}."
)
return None
return self.__compute_ik_client.call_async(self.__compute_ik_req)

def reset_new_joint_state_checker(self):
"""