Merge pull request #57 from pollen-robotics/56-more-control-over-ik-behaviors-from-grpc-level

56 more control over ik behaviors from grpc level

Author: RemiFabre

pollen_kdl_kinematics/pollen_kdl_kinematics/pollen_kdl_kinematics_node.py

@@ -8,6 +8,7 @@
 import rclpy
 from geometry_msgs.msg import Pose, PoseStamped
 from pollen_msgs.srv import GetForwardKinematics, GetInverseKinematics
+from pollen_msgs.msg import IKRequest, ReachabilityState
 from rclpy.lifecycle import LifecycleNode, State, TransitionCallbackReturn
 from rclpy.qos import (HistoryPolicy, QoSDurabilityPolicy, QoSProfile,
                        ReliabilityPolicy)
@@ -21,7 +22,6 @@
                                        limit_orbita3d_joints,
                                        limit_orbita3d_joints_wrist,
                                        limit_theta_to_interval,
-                                       tend_to_prefered_theta,
                                        get_best_theta_to_current_joints,)
 from scipy.spatial.transform import Rotation
 from sensor_msgs.msg import JointState
@@ -56,7 +56,9 @@ def __init__(self):
 
         self.chain, self.fk_solver, self.ik_solver = {}, {}, {}
         self.fk_srv, self.ik_srv = {}, {}
+        self.reachability_pub = {}
         self.target_sub, self.averaged_target_sub = {}, {}
+        self.ik_target_sub = {}
         self.averaged_pose = {}
         self.max_joint_vel = {}
 
@@ -129,10 +131,18 @@ def __init__(self):
                     qos_profile=5,
                 )
 
+                self.reachability_pub[arm] = self.create_publisher(
+                    msg_type=ReachabilityState,
+                    topic=f"/{arm}_reachability_states",
+                    qos_profile=10,
+                )
+
                 if arm.startswith("l"):
                     q0 = [0.0, np.pi / 2, 0.0, -np.pi / 2, 0.0, 0.0, 0.0]
                 else:
                     q0 = [0.0, -np.pi / 2, 0.0, -np.pi / 2, 0.0, 0.0, 0.0]
+
+                # Keeping both on _target_pose mechanisms for compatibility reasons
                 self.target_sub[arm] = self.create_subscription(
                     msg_type=PoseStamped,
                     topic=f"/{arm}/target_pose",
@@ -149,6 +159,18 @@ def __init__(self):
                 )
                 self.logger.info(f'Adding subscription on "{self.target_sub[arm].topic}"...')
 
+                self.ik_target_sub[arm] = self.create_subscription(
+                    msg_type=IKRequest,
+                    topic=f"/{arm}/ik_target_pose",
+                    qos_profile=high_freq_qos_profile,
+                    callback=partial(
+                        self.on_ik_target_pose,
+                        name=arm,
+                        forward_publisher=forward_position_pub,
+                    ),
+                )
+                self.logger.info(f'Adding subscription on "{self.ik_target_sub[arm].topic}"...')
+
                 self.averaged_target_sub[arm] = self.create_subscription(
                     msg_type=PoseStamped,
                     topic=f"/{arm}/averaged_target_pose",
@@ -371,7 +393,7 @@ def inverse_kinematics_srv(
                 M,
                 "discrete",
                 current_joints=current_joints,
-                interval_limit=[-np.pi, np.pi],
+                constrained_mode="unconstrained",
                 current_pose=current_pose
             )
             # # self.logger.info(M)
@@ -394,8 +416,65 @@ def inverse_kinematics_srv(
 
         return response
 
+    def on_ik_target_pose(self, msg: IKRequest, name, forward_publisher):
+        M = ros_pose_to_matrix(msg.pose.pose)
+        constrained_mode = msg.constrained_mode
+        continuous_mode = msg.continuous_mode
+        preferred_theta = msg.preferred_theta
+        d_theta_max = msg.d_theta_max
+        order_id = msg.order_id
+
+        if continuous_mode == "undefined":
+            continuous_mode = "continuous"
+
+        if constrained_mode == "undefined":
+            constrained_mode = "unconstrained"
+
+        # if constrained_mode == "low_elbow":
+        #     interval_limit = [-4 * np.pi / 5, 0]
+        # self.logger.info(f"Preferred theta: {preferred_theta}")
+        # self.logger.info(f"Continuous mode: {continuous_mode}")
+        # self.logger.info(f"Constrained mode: {constrained_mode}")
+        # self.logger.info(f"Order ID: {order_id}")
+        # self.logger.info(f"Max d_theta: {d_theta_max}")
+
+        if "arm" in name:
+            current_joints = self.get_current_position(self.chain[name])
+            error, current_pose = forward_kinematics(
+                self.fk_solver[name],
+                current_joints,
+                self.chain[name].getNrOfJoints(),
+            )
+
+            sol, is_reachable, state = self.control_ik.symbolic_inverse_kinematics(
+                name,
+                M,
+                continuous_mode,
+                current_joints=current_joints,
+                constrained_mode=constrained_mode,
+                current_pose=current_pose,
+                d_theta_max=d_theta_max,
+                preferred_theta=preferred_theta,
+            )
+        else:
+            self.logger.error("IK target pose should be only for the arms")
+            raise ValueError("IK target pose should be only for the arms")
+
+        msg = Float64MultiArray()
+        msg.data = sol
+        forward_publisher.publish(msg)
+        reachability_msg = ReachabilityState()
+        reachability_msg.header.stamp = self.get_clock().now().to_msg()
+        reachability_msg.header.frame_id = "torso"
+        reachability_msg.is_reachable = is_reachable
+        reachability_msg.state = state
+        reachability_msg.order_id = order_id
+        self.reachability_pub[name].publish(reachability_msg)
+
+
     def on_target_pose(self, msg: PoseStamped, name, q0, forward_publisher):
         M = ros_pose_to_matrix(msg.pose)
+
         if "arm" in name:
             current_joints = self.get_current_position(self.chain[name])
             error, current_pose = forward_kinematics(
@@ -408,7 +487,7 @@ def on_target_pose(self, msg: PoseStamped, name, q0, forward_publisher):
                 M,
                 "continuous",
                 current_joints=current_joints,
-                interval_limit=[-4 * np.pi / 5, 0],
+                constrained_mode="low_elbow",  # "unconstrained"
                 current_pose=current_pose
             )
         else:

pollen_msgs/CMakeLists.txt

@@ -26,6 +26,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/GotoFeedback.msg"
   "msg/GotoRequest.msg"
   "msg/GotoResult.msg"
+  "msg/IKRequest.msg"
+  "msg/ReachabilityState.msg"
 
   "action/Goto.action"
 

pollen_msgs/msg/IKRequest.msg

@@ -0,0 +1,23 @@
+# 6d goal pose
+geometry_msgs/PoseStamped pose
+
+# Current options are "unconstrained" and "low_elbow"
+string constrained_mode
+
+# Value in [-pi, pi], angle of the elbow that the IK will try to obtain
+# The value should be the same for both arms - IK will adapt this value for the left arm
+# Examples : 
+# -4pi/6 - Default value
+# -5pi/6 - with this value the elbows are a bit higher
+
+float64 preferred_theta
+
+# When in continuous mode this is the maximum amount of rads that the elbow can rotate on the reachability circle
+# Common value : 0.01
+float64 d_theta_max
+
+# Current options are "continuous" and "discrete"
+string continuous_mode
+
+# When present this value must be unic for each message and it will be used to identify reachability information for a given request
+int64 order_id

pollen_msgs/msg/ReachabilityState.msg

@@ -0,0 +1,8 @@
+std_msgs/Header header
+
+#it will be used to identify reachability information for a given request
+int64 order_id
+
+bool is_reachable
+
+string state