add get_jacobian and option for making 'not reached' being failure or…

… not

arm_robots/src/arm_robots/hdt_michigan.py

@@ -97,6 +97,15 @@ def threshold_velocities(self, joint_names, velocities):
 
         return fixed_velocities
 
+    def send_velocity_joint_command(self, joint_names: List[str], velocities):
+        trajectory_point = JointTrajectoryPoint()
+        trajectory_point.velocities = velocities
+        # set the target position to be just ahead of the current position, in the direction based on sign of velocity
+        offset = np.sign(velocities) * MAX_JOINT_ANGLE_DELTA_RAD * 0.9
+        current_joint_command = self.get_joint_positions(joint_names)
+        trajectory_point.positions = np.array(current_joint_command) + offset
+        return self.send_joint_command(joint_names, trajectory_point)
+
     def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint):
         current_joint_command = self.get_joint_positions(joint_names)
         distance = np.linalg.norm(np.array(current_joint_command) - np.array(trajectory_point.positions))
@@ -209,6 +218,12 @@ def is_left_gripper_closed(self):
     def is_right_gripper_closed(self):
         return self.is_gripper_closed('right')
 
+    def get_current_right_tool_jacobian(self):
+        return self.get_current_jacobian(self.right_arm_group, self.right_tool_name)
+
+    def get_current_left_tool_jacobian(self):
+        return self.get_current_jacobian(self.left_arm_group, self.left_tool_name)
+
 
 def estimated_torques(joint_state: JointState, robot: Val):
     group_name1 = "left_side"

arm_robots/src/arm_robots/robot.py

@@ -1,12 +1,11 @@
 #! /usr/bin/env python
+import warnings
 from typing import List, Union, Tuple, Callable, Optional, Dict
 
 import numpy as np
 import pyjacobian_follower
 from matplotlib import colors
 
-import warnings
-
 with warnings.catch_warnings():
     warnings.simplefilter("ignore", category=RuntimeWarning)
     import moveit_commander
@@ -22,7 +21,7 @@
     ExecutionResult, is_empty_trajectory, merge_joint_state_and_scene_msg
 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryFeedback, FollowJointTrajectoryResult, \
     FollowJointTrajectoryGoal
-from geometry_msgs.msg import Point, Pose, Quaternion, Vector3
+from geometry_msgs.msg import Point, Pose, Quaternion, Vector3, PointStamped
 from moveit_msgs.msg import RobotTrajectory, DisplayRobotState, ObjectColor, RobotState, PlanningScene, \
     DisplayTrajectory
 from rosgraph.names import ns_join
@@ -66,9 +65,9 @@ def __init__(self,
                  display_goals: bool = True,
                  force_trigger: float = 9.0,
                  jacobian_follower: Optional[pyjacobian_follower.JacobianFollower] = None,
-                 jacobian_not_reached_is_failure: Optional[bool] = True):
+                 jacobian_target_not_reached_is_failure: Optional[bool] = True):
         super().__init__(robot_namespace, robot_description)
-        self.jacobian_not_reached_is_failure = jacobian_not_reached_is_failure
+        self.jacobian_target_not_reached_is_failure = jacobian_target_not_reached_is_failure
         self._max_velocity_scale_factor = 0.1
         self.stored_tool_orientations = None
         self.raise_on_failure = raise_on_failure
@@ -390,7 +389,11 @@ def follow_jacobian_to_position_from_scene_and_state(self,
             max_acceleration_scaling_factor=0.1,
         )
 
-        planning_success = reached
+        if self.jacobian_target_not_reached_is_failure:
+            planning_success = reached
+        else:
+            planning_success = True
+
         planning_result = PlanningResult(success=planning_success, plan=robot_trajectory_msg)
         if self.raise_on_failure and not planning_success:
             raise RobotPlanningError(f"Tried to execute a jacobian action which could not be reached")
@@ -430,7 +433,7 @@ def follow_jacobian_to_position(self,
             max_velocity_scaling_factor=vel_scaling,
             max_acceleration_scaling_factor=0.1,
         )
-        if self.jacobian_not_reached_is_failure:
+        if self.jacobian_target_not_reached_is_failure:
             planning_success = reached
         else:
             planning_success = True
@@ -520,16 +523,17 @@ def display_robot_state(self, robot_state: RobotState, label: str, color: Option
 
         display_robot_state_pub.publish(display_robot_state_msg)
 
-    def display_goal_position(self, point: Point):
+    def display_goal_position(self, point: PointStamped):
         m = Marker()
         m.header.stamp = rospy.Time.now()
+        m.header.frame_id = point.header.frame_id
         m.id = 0
         m.action = Marker.ADD
         m.action = Marker.SPHERE
         m.color = ColorRGBA(r=0, g=1, b=0, a=1)
         s = 0.02
         m.scale = Vector3(x=s, y=s, z=s)
-        m.pose.position = point
+        m.pose.position = point.point
         m.pose.orientation.w = 1
         self.display_goal_position_pub.publish(m)
 
@@ -550,3 +554,13 @@ def get_state(self, group_name: str = None):
 
     def estimated_torques(self, robot_state: RobotState, group, wrenches=None):
         return self.jacobian_follower.estimated_torques(robot_state, group, wrenches)
+
+    def get_current_jacobian(self, group_name: str, link_name: str):
+        current_joint_positions = self.get_joint_positions(self.get_joint_names(group_name))
+        return self.jacobian_follower.get_jacobian(group_name, link_name, current_joint_positions)
+
+    def get_jacobian(self, group_name: str, joint_positions):
+        return self.jacobian_follower.get_jacobian(group_name, joint_positions)
+
+    def get_base_link(self, group_name: str):
+        return self.jacobian_follower.get_base_link(group_name)

jacobian_follower/include/jacobian_follower/jacobian_follower.hpp

@@ -255,6 +255,11 @@ class JacobianFollower {
 
   moveit_msgs::PlanningScene get_scene() const;
 
+  std::tuple<Eigen::MatrixXd, bool> getJacobian(std::string const &group_name, std::string const &link_name,
+                                                std::vector<double> const &joint_positions);
+
+  std::string getBaseLink(std::string const& group_name) const;
+
   template <typename A, typename B>
   void validateNamesAndPositions(const std::vector<A> &joint_names, const std::vector<B> &joint_positions) const {
     if (joint_names.size() != joint_positions.size()) {

jacobian_follower/src/jacobian_follower/bindings.cpp

@@ -75,6 +75,8 @@ PYBIND11_MODULE(pyjacobian_follower, m) {
       .def("is_collision_checking", &JacobianFollower::isCollisionChecking)
       .def("estimated_torques", &JacobianFollower::estimatedTorques)
       .def("get_scene", &JacobianFollower::get_scene)
+      .def("get_jacobian", &JacobianFollower::getJacobian)
+      .def("get_base_link", &JacobianFollower::getBaseLink)
       //
       ;
 }

jacobian_follower/src/jacobian_follower/jacobian_follower.cpp

@@ -1054,3 +1054,21 @@ moveit_msgs::PlanningScene JacobianFollower::get_scene() const {
   scene_monitor_->getPlanningScene()->getPlanningSceneMsg(msg);
   return msg;
 }
+
+std::tuple<Eigen::MatrixXd, bool> JacobianFollower::getJacobian(std::string const &group_name,
+                                                                std::string const &link_name,
+                                                                std::vector<double> const &joint_positions) {
+  auto const jmg = model_->getJointModelGroup(group_name);
+  robot_state::RobotState state(model_);
+  state.setVariablePositions(jmg->getActiveJointModelNames(), joint_positions);
+
+  Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
+  Eigen::MatrixXd jacobian;
+  auto const success = state.getJacobian(jmg, state.getLinkModel(link_name), reference_point_position, jacobian);
+  return std::tuple{jacobian, success};
+}
+
+std::string JacobianFollower::getBaseLink(std::string const &group_name) const {
+  auto const jmg = model_->getJointModelGroup(group_name);
+  return jmg->getCommonRoot()->getParentLinkModel()->getParentLinkModel()->getName();
+}