Python interface improvements. Fix moveit#1966, add enforceBounds (moveit#2356)

* implement get state and get state bounded

* add enforceBounds

Co-authored-by: Peter <pmitrano@storm20.04>

Author: PeterMitrano

moveit_commander/src/moveit_commander/move_group.py

@@ -156,6 +156,20 @@ def set_start_state(self, msg):
         """
         self._g.set_start_state(conversions.msg_to_string(msg))
 
+    def get_current_state_bounded(self):
+        """ Get the current state of the robot bounded."""
+        s = RobotState()
+        c_str = self._g.get_current_state_bounded()
+        conversions.msg_from_string(s, c_str)
+        return s
+
+    def get_current_state(self):
+        """ Get the current state of the robot."""
+        s = RobotState()
+        c_str = self._g.get_current_state()
+        conversions.msg_from_string(s, c_str)
+        return s
+
     def get_joint_value_target(self):
         return self._g.get_joint_value_target()
 
@@ -230,7 +244,6 @@ def set_joint_value_target(self, arg1, arg2=None, arg3=None):
         else:
             raise MoveItCommanderException("Unsupported argument of type %s" % type(arg1))
 
-
     def set_rpy_target(self, rpy, end_effector_link=""):
         """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
@@ -610,3 +623,9 @@ def get_jacobian_matrix(self, joint_values, reference_point=None):
         """ Get the jacobian matrix of the group as a list"""
         return self._g.get_jacobian_matrix(joint_values, [0.0, 0.0, 0.0] if reference_point is None else reference_point)
 
+    def enforce_bounds(self, robot_state_msg):
+        """ Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """
+        s = RobotState()
+        c_str = self._g.enforce_bounds(conversions.msg_to_string(robot_state_msg))
+        conversions.msg_from_string(s, c_str)
+        return s

moveit_commander/test/python_moveit_commander.py

@@ -35,11 +35,15 @@
 # Author: William Baker
 
 import unittest
+
+import genpy
 import numpy as np
 import rospy
 import rostest
 import os
 
+from moveit_msgs.msg import RobotState
+
 from moveit_commander import RobotCommander, PlanningSceneInterface
 
 
@@ -55,6 +59,31 @@ def setUpClass(self):
     def tearDown(self):
         pass
 
+    def test_enforce_bounds_empty_state(self):
+        in_msg = RobotState()
+        with self.assertRaises(genpy.DeserializationError):
+            self.group.enforce_bounds(in_msg)
+
+    def test_enforce_bounds(self):
+        in_msg = RobotState()
+        in_msg.joint_state.header.frame_id = 'base_link'
+        in_msg.joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
+        in_msg.joint_state.position = [0] * 6
+        in_msg.joint_state.position[0] = 1000
+
+        out_msg = self.group.enforce_bounds(in_msg)
+
+        self.assertEqual(in_msg.joint_state.position[0], 1000)
+        self.assertLess(out_msg.joint_state.position[0], 1000)
+
+    def test_get_current_state(self):
+        expected_state = RobotState()
+        expected_state.joint_state.header.frame_id = 'base_link'
+        expected_state.multi_dof_joint_state.header.frame_id = 'base_link'
+        expected_state.joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
+        expected_state.joint_state.position = [0] * 6
+        self.assertEqual(self.group.get_current_state(), expected_state)
+
     def check_target_setting(self, expect, *args):
         if len(args) == 0:
             args = [expect]

moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp

@@ -342,6 +342,14 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     return output;
   }
 
+  py_bindings_tools::ByteString getCurrentStatePython()
+  {
+    moveit::core::RobotStatePtr current_state = getCurrentState();
+    moveit_msgs::RobotState state_message;
+    moveit::core::robotStateToRobotStateMsg(*current_state, state_message);
+    return py_bindings_tools::serializeMsg(state_message);
+  }
+
   void setStartStatePython(const py_bindings_tools::ByteString& msg_str)
   {
     moveit_msgs::RobotState msg;
@@ -602,6 +610,24 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     state.setJointGroupPositions(group, v);
     return state.getJacobian(group, Eigen::Map<Eigen::Vector3d>(&ref[0]));
   }
+
+  py_bindings_tools::ByteString enforceBoundsPython(const py_bindings_tools::ByteString& msg_str)
+  {
+    moveit_msgs::RobotState state_msg;
+    py_bindings_tools::deserializeMsg(msg_str, state_msg);
+    moveit::core::RobotState state(getRobotModel());
+    if (moveit::core::robotStateMsgToRobotState(state_msg, state, true))
+    {
+      state.enforceBounds();
+      moveit::core::robotStateToRobotStateMsg(state, state_msg);
+      return py_bindings_tools::serializeMsg(state_msg);
+    }
+    else
+    {
+      ROS_ERROR("Unable to convert RobotState message to RobotState instance.");
+      return py_bindings_tools::ByteString("");
+    }
+  }
 };
 
 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getJacobianMatrixOverloads, getJacobianMatrixPython, 1, 2)
@@ -742,8 +768,10 @@ static void wrap_move_group_interface()
   move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython);
   move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython);
   move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython);
+  move_group_interface_class.def("get_current_state", &MoveGroupInterfaceWrapper::getCurrentStatePython);
   move_group_interface_class.def("get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython,
                                  getJacobianMatrixOverloads());
+  move_group_interface_class.def("enforce_bounds", &MoveGroupInterfaceWrapper::enforceBoundsPython);
 }
 }  // namespace planning_interface
 }  // namespace moveit