diff --git a/victor_hardware_interface/.clang-format b/victor_hardware_interface/.clang-format new file mode 100644 index 00000000..6b72b54b --- /dev/null +++ b/victor_hardware_interface/.clang-format @@ -0,0 +1,168 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: false +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: WithoutElse +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 120 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DerivePointerAlignment: true +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^' + Priority: 2 + SortPriority: 0 + - Regex: '^<.*\.h>' + Priority: 1 + SortPriority: 0 + - Regex: '^<.*' + Priority: 2 + SortPriority: 0 + - Regex: '.*' + Priority: 3 + SortPriority: 0 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IncludeIsMainSourceRegex: '' +IndentCaseLabels: true +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - 'c++' + - 'C++' + CanonicalDelimiter: '' + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + CanonicalDelimiter: '' + BasedOnStyle: google +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +Standard: Auto +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never +... + diff --git a/victor_hardware_interface/include/victor_hardware_interface/minimal_arm_wrapper_interface.hpp b/victor_hardware_interface/include/victor_hardware_interface/minimal_arm_wrapper_interface.hpp index dbcefb88..b031f4fb 100644 --- a/victor_hardware_interface/include/victor_hardware_interface/minimal_arm_wrapper_interface.hpp +++ b/victor_hardware_interface/include/victor_hardware_interface/minimal_arm_wrapper_interface.hpp @@ -53,9 +53,9 @@ namespace victor_hardware_interface MinimalArmWrapperInterface( ros::NodeHandle& nh, - const std::shared_ptr& send_lcm_ptr, - const std::shared_ptr& recv_lcm_ptr, - const std::string& cartesian_control_frame, + std::shared_ptr send_lcm_ptr, + std::shared_ptr recv_lcm_ptr, + std::string cartesian_control_frame, const double set_control_mode_timeout, // ROS Topics const std::string& motion_command_topic, diff --git a/victor_hardware_interface/scripts/manual_motion.py b/victor_hardware_interface/scripts/manual_motion.py index df497c94..93e58c5f 100755 --- a/victor_hardware_interface/scripts/manual_motion.py +++ b/victor_hardware_interface/scripts/manual_motion.py @@ -16,15 +16,15 @@ from victor_hardware_interface_msgs.msg import MotionCommand, MotionStatus, ControlModeParameters, ControlMode from threading import Lock from arc_utilities import ros_helpers as rh - +from arc_utilities.listener import Listener joint_names = ['joint_' + str(i) for i in range(1, 8)] -hard_limits_deg = np.array([170.0, 120.0, 170.0, 120.0, 170.0, 120.0, 175.0]) +hard_limits_deg = np.array([170.0, 120.0, 170.0, 120.0, 170.0, 120.0, 175.0]) safety_deg = 30.0 # Stay far away from joint hard limits -joint_lower = -(hard_limits_deg - safety_deg) * np.pi/180 -joint_upper = (hard_limits_deg - safety_deg) * np.pi/180 +joint_lower = -(hard_limits_deg - safety_deg) * np.pi / 180 +joint_upper = (hard_limits_deg - safety_deg) * np.pi / 180 class ManualMotion: @@ -35,7 +35,7 @@ def __init__(self, arm_name): self.threshold = 0.05 self.arrive_threshold = 0.02 - + self.low_pass_tau = 0.01 # seconds self.prev_time = None self.lock = Lock() @@ -47,7 +47,7 @@ def callback_update(self, msg): self.run_lowpass(msg) def run_lowpass(self, msg): - + if self.prev_time is None: self.prev_time = time.time() @@ -61,12 +61,11 @@ def run_lowpass(self, msg): assert False # Update our internal copy and calculate error for later thresholding - meas = np.array([getattr(msg.measured_joint_position, joint) for joint in joint_names]) - cmd = np.array([getattr(msg.commanded_joint_position, joint) for joint in joint_names]) + meas = np.array([getattr(msg.measured_joint_position, joint) for joint in joint_names]) + cmd = np.array([getattr(msg.commanded_joint_position, joint) for joint in joint_names]) err = np.linalg.norm(meas - cmd) self.last_pos = meas - if err > self.threshold: # Robot is far enough away from setpoint that we assume someone is pushing it self.follow = True @@ -92,8 +91,8 @@ def run_lowpass(self, msg): for idx in range(len(joint_names)): setattr(cmd_msg.joint_position, joint_names[idx], new_cmd[idx]) self.pub.publish(cmd_msg) - - + + def print_joints(left, right): """Print nicely to the terminal so joint values can be copied""" rospy.loginfo("Joint angles are: ") @@ -106,33 +105,30 @@ def print_joints(left, right): if __name__ == "__main__": rospy.init_node("manual_motion") - control_mode_params = vu.get_joint_impedance_params(vu.Stiffness.MEDIUM) use_left_arm = rospy.get_param("~use_left_arm", True) use_right_arm = rospy.get_param("~use_right_arm", True) - - - if(use_left_arm): + if use_left_arm: print("initializing left arm ...") sys.stdout.flush() - l_cm = rh.Listener("left_arm/control_mode_status", ControlModeParameters) + l_cm = Listener("left_arm/control_mode_status", ControlModeParameters) cur_mode = l_cm.get(block_until_data=True) control_mode_params.joint_path_execution_params.joint_relative_velocity = cur_mode.joint_path_execution_params.joint_relative_velocity control_mode_params.joint_path_execution_params.joint_relative_acceleration = cur_mode.joint_path_execution_params.joint_relative_acceleration - + result = vu.send_new_control_mode("left_arm", control_mode_params) while not result.success: result = vu.send_new_control_mode("left_arm", control_mode_params) print("done") else: - print("not using left arm") + print("not using left arm") - if(use_right_arm): + if use_right_arm: print("initializing right arm ...") - r_cm = rh.Listener("right_arm/control_mode_status", ControlModeParameters) + r_cm = Listener("right_arm/control_mode_status", ControlModeParameters) cur_mode = r_cm.get(block_until_data=True) control_mode_params.joint_path_execution_params.joint_relative_velocity = cur_mode.joint_path_execution_params.joint_relative_velocity control_mode_params.joint_path_execution_params.joint_relative_acceleration = cur_mode.joint_path_execution_params.joint_relative_acceleration @@ -143,18 +139,15 @@ def print_joints(left, right): result = vu.send_new_control_mode("right_arm", control_mode_params) print("done") else: - print("not using right arm") - + print("not using right arm") left = None right = None - if(use_left_arm): + if use_left_arm: left = ManualMotion("left_arm") - if(use_right_arm): + if use_right_arm: right = ManualMotion("right_arm") - - rospy.on_shutdown(lambda: print_joints(left, right)) print("Running") diff --git a/victor_hardware_interface/src/victor_hardware_interface/iiwa_hardware_interface.cpp b/victor_hardware_interface/src/victor_hardware_interface/iiwa_hardware_interface.cpp index 0fa260a3..a61ab831 100644 --- a/victor_hardware_interface/src/victor_hardware_interface/iiwa_hardware_interface.cpp +++ b/victor_hardware_interface/src/victor_hardware_interface/iiwa_hardware_interface.cpp @@ -3,342 +3,293 @@ #include namespace msgs = victor_hardware_interface_msgs; -namespace victor_hardware_interface -{ - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // LCM to ROS and ROS to LCM message converters - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - msgs::JointValueQuantity jvqLcmToRos(const joint_value_quantity& lcm_jvq) - { - msgs::JointValueQuantity ros_jvq; - ros_jvq.joint_1 = lcm_jvq.joint_1; - ros_jvq.joint_2 = lcm_jvq.joint_2; - ros_jvq.joint_3 = lcm_jvq.joint_3; - ros_jvq.joint_4 = lcm_jvq.joint_4; - ros_jvq.joint_5 = lcm_jvq.joint_5; - ros_jvq.joint_6 = lcm_jvq.joint_6; - ros_jvq.joint_7 = lcm_jvq.joint_7; - return ros_jvq; - } - - joint_value_quantity jvqRosToLcm(const msgs::JointValueQuantity& ros_jvq) - { - joint_value_quantity lcm_jvq; - lcm_jvq.joint_1 = ros_jvq.joint_1; - lcm_jvq.joint_2 = ros_jvq.joint_2; - lcm_jvq.joint_3 = ros_jvq.joint_3; - lcm_jvq.joint_4 = ros_jvq.joint_4; - lcm_jvq.joint_5 = ros_jvq.joint_5; - lcm_jvq.joint_6 = ros_jvq.joint_6; - lcm_jvq.joint_7 = ros_jvq.joint_7; - return lcm_jvq; - } - - - msgs::CartesianValueQuantity cvqLcmToRos(const cartesian_value_quantity& lcm_cvq) - { - msgs::CartesianValueQuantity ros_cvq; - ros_cvq.x = lcm_cvq.x; - ros_cvq.y = lcm_cvq.y; - ros_cvq.z = lcm_cvq.z; - ros_cvq.a = lcm_cvq.a; - ros_cvq.b = lcm_cvq.b; - ros_cvq.c = lcm_cvq.c; - return ros_cvq; - } - - cartesian_value_quantity cvqRosToLcm(const msgs::CartesianValueQuantity& ros_cvq) - { - cartesian_value_quantity lcm_cvq; - lcm_cvq.x = ros_cvq.x; - lcm_cvq.y = ros_cvq.y; - lcm_cvq.z = ros_cvq.z; - lcm_cvq.a = ros_cvq.a; - lcm_cvq.b = ros_cvq.b; - lcm_cvq.c = ros_cvq.c; - return lcm_cvq; - } - - - geometry_msgs::Pose poseLcmToRos(const cartesian_pose& lcm_pose) - { - geometry_msgs::Pose ros_pose; - ros_pose.position.x = lcm_pose.xt; - ros_pose.position.y = lcm_pose.yt; - ros_pose.position.z = lcm_pose.zt; - ros_pose.orientation.w = lcm_pose.wr; - ros_pose.orientation.x = lcm_pose.xr; - ros_pose.orientation.y = lcm_pose.yr; - ros_pose.orientation.z = lcm_pose.zr; - return ros_pose; - } - - cartesian_pose poseRosToLcm(const geometry_msgs::Pose& ros_pose) - { - cartesian_pose lcm_pose; - lcm_pose.xt = ros_pose.position.x; - lcm_pose.yt = ros_pose.position.y; - lcm_pose.zt = ros_pose.position.z; - lcm_pose.wr = ros_pose.orientation.w; - lcm_pose.xr = ros_pose.orientation.x; - lcm_pose.yr = ros_pose.orientation.y; - lcm_pose.zr = ros_pose.orientation.z; - return lcm_pose; - } - - - msgs::JointImpedanceParameters jointImpedanceParamsLcmToRos(const joint_impedance_parameters& lcm_jip) - { - msgs::JointImpedanceParameters ros_jip; - ros_jip.joint_damping = jvqLcmToRos(lcm_jip.joint_damping); - ros_jip.joint_stiffness = jvqLcmToRos(lcm_jip.joint_stiffness); - return ros_jip; - } - - joint_impedance_parameters jointImpedanceParamsRosToLcm(const msgs::JointImpedanceParameters& ros_jip) - { - joint_impedance_parameters lcm_jip; - lcm_jip.joint_damping = jvqRosToLcm(ros_jip.joint_damping); - lcm_jip.joint_stiffness = jvqRosToLcm(ros_jip.joint_stiffness); - return lcm_jip; - } - +namespace victor_hardware_interface { +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// LCM to ROS and ROS to LCM message converters +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +msgs::JointValueQuantity jvqLcmToRos(const joint_value_quantity& lcm_jvq) { + msgs::JointValueQuantity ros_jvq; + ros_jvq.joint_1 = lcm_jvq.joint_1; + ros_jvq.joint_2 = lcm_jvq.joint_2; + ros_jvq.joint_3 = lcm_jvq.joint_3; + ros_jvq.joint_4 = lcm_jvq.joint_4; + ros_jvq.joint_5 = lcm_jvq.joint_5; + ros_jvq.joint_6 = lcm_jvq.joint_6; + ros_jvq.joint_7 = lcm_jvq.joint_7; + return ros_jvq; +} - msgs::CartesianImpedanceParameters cartesianImpedanceParamsLcmToRos(const cartesian_impedance_parameters& lcm_cip) - { - msgs::CartesianImpedanceParameters ros_cip; - ros_cip.cartesian_damping = cvqLcmToRos(lcm_cip.cartesian_damping); - ros_cip.nullspace_damping = lcm_cip.nullspace_damping; - ros_cip.cartesian_stiffness = cvqLcmToRos(lcm_cip.cartesian_stiffness); - ros_cip.nullspace_stiffness = lcm_cip.nullspace_stiffness; - return ros_cip; - } +joint_value_quantity jvqRosToLcm(const msgs::JointValueQuantity& ros_jvq) { + joint_value_quantity lcm_jvq; + lcm_jvq.joint_1 = ros_jvq.joint_1; + lcm_jvq.joint_2 = ros_jvq.joint_2; + lcm_jvq.joint_3 = ros_jvq.joint_3; + lcm_jvq.joint_4 = ros_jvq.joint_4; + lcm_jvq.joint_5 = ros_jvq.joint_5; + lcm_jvq.joint_6 = ros_jvq.joint_6; + lcm_jvq.joint_7 = ros_jvq.joint_7; + return lcm_jvq; +} - cartesian_impedance_parameters cartesianImpedanceParamsRosToLcm(const msgs::CartesianImpedanceParameters& ros_cip) - { - cartesian_impedance_parameters lcm_cip; - lcm_cip.cartesian_damping = cvqRosToLcm(ros_cip.cartesian_damping); - lcm_cip.nullspace_damping = ros_cip.nullspace_damping; - lcm_cip.cartesian_stiffness = cvqRosToLcm(ros_cip.cartesian_stiffness); - lcm_cip.nullspace_stiffness = ros_cip.nullspace_stiffness; - return lcm_cip; - } +msgs::CartesianValueQuantity cvqLcmToRos(const cartesian_value_quantity& lcm_cvq) { + msgs::CartesianValueQuantity ros_cvq; + ros_cvq.x = lcm_cvq.x; + ros_cvq.y = lcm_cvq.y; + ros_cvq.z = lcm_cvq.z; + ros_cvq.a = lcm_cvq.a; + ros_cvq.b = lcm_cvq.b; + ros_cvq.c = lcm_cvq.c; + return ros_cvq; +} +cartesian_value_quantity cvqRosToLcm(const msgs::CartesianValueQuantity& ros_cvq) { + cartesian_value_quantity lcm_cvq; + lcm_cvq.x = ros_cvq.x; + lcm_cvq.y = ros_cvq.y; + lcm_cvq.z = ros_cvq.z; + lcm_cvq.a = ros_cvq.a; + lcm_cvq.b = ros_cvq.b; + lcm_cvq.c = ros_cvq.c; + return lcm_cvq; +} - msgs::JointPathExecutionParameters jointPexpLcmToRos(const joint_path_execution_parameters& lcm_pexp) - { - msgs::JointPathExecutionParameters ros_pexp; - ros_pexp.joint_relative_acceleration = lcm_pexp.joint_relative_acceleration; - ros_pexp.joint_relative_velocity = lcm_pexp.joint_relative_velocity; - ros_pexp.override_joint_acceleration = lcm_pexp.override_joint_acceleration; - return ros_pexp; - } +geometry_msgs::Pose poseLcmToRos(const cartesian_pose& lcm_pose) { + geometry_msgs::Pose ros_pose; + ros_pose.position.x = lcm_pose.xt; + ros_pose.position.y = lcm_pose.yt; + ros_pose.position.z = lcm_pose.zt; + ros_pose.orientation.w = lcm_pose.wr; + ros_pose.orientation.x = lcm_pose.xr; + ros_pose.orientation.y = lcm_pose.yr; + ros_pose.orientation.z = lcm_pose.zr; + return ros_pose; +} - joint_path_execution_parameters jointPexpRosToLcm(const msgs::JointPathExecutionParameters& ros_pexp) - { - joint_path_execution_parameters lcm_pexp; - lcm_pexp.joint_relative_acceleration = ros_pexp.joint_relative_acceleration; - lcm_pexp.joint_relative_velocity = ros_pexp.joint_relative_velocity; - lcm_pexp.override_joint_acceleration = ros_pexp.override_joint_acceleration; - return lcm_pexp; - } +cartesian_pose poseRosToLcm(const geometry_msgs::Pose& ros_pose) { + cartesian_pose lcm_pose; + lcm_pose.xt = ros_pose.position.x; + lcm_pose.yt = ros_pose.position.y; + lcm_pose.zt = ros_pose.position.z; + lcm_pose.wr = ros_pose.orientation.w; + lcm_pose.xr = ros_pose.orientation.x; + lcm_pose.yr = ros_pose.orientation.y; + lcm_pose.zr = ros_pose.orientation.z; + return lcm_pose; +} +msgs::JointImpedanceParameters jointImpedanceParamsLcmToRos(const joint_impedance_parameters& lcm_jip) { + msgs::JointImpedanceParameters ros_jip; + ros_jip.joint_damping = jvqLcmToRos(lcm_jip.joint_damping); + ros_jip.joint_stiffness = jvqLcmToRos(lcm_jip.joint_stiffness); + return ros_jip; +} - msgs::CartesianPathExecutionParameters cartesianPexpLcmToRos(const cartesian_path_execution_parameters& lcm_pexp) - { - msgs::CartesianPathExecutionParameters ros_pexp; - ros_pexp.max_velocity = cvqLcmToRos(lcm_pexp.max_velocity); - ros_pexp.max_acceleration = cvqLcmToRos(lcm_pexp.max_acceleration); - ros_pexp.max_nullspace_velocity = lcm_pexp.max_nullspace_velocity; - ros_pexp.max_nullspace_acceleration = lcm_pexp.max_nullspace_acceleration; - return ros_pexp; - } +joint_impedance_parameters jointImpedanceParamsRosToLcm(const msgs::JointImpedanceParameters& ros_jip) { + joint_impedance_parameters lcm_jip; + lcm_jip.joint_damping = jvqRosToLcm(ros_jip.joint_damping); + lcm_jip.joint_stiffness = jvqRosToLcm(ros_jip.joint_stiffness); + return lcm_jip; +} - cartesian_path_execution_parameters cartesianPexpRosToLcm(const msgs::CartesianPathExecutionParameters& ros_pexp) - { - cartesian_path_execution_parameters lcm_pexp; - lcm_pexp.max_velocity = cvqRosToLcm(ros_pexp.max_velocity); - lcm_pexp.max_acceleration = cvqRosToLcm(ros_pexp.max_acceleration); - lcm_pexp.max_nullspace_velocity = ros_pexp.max_nullspace_velocity; - lcm_pexp.max_nullspace_acceleration = ros_pexp.max_nullspace_acceleration; - return lcm_pexp; - } +msgs::CartesianImpedanceParameters cartesianImpedanceParamsLcmToRos(const cartesian_impedance_parameters& lcm_cip) { + msgs::CartesianImpedanceParameters ros_cip; + ros_cip.cartesian_damping = cvqLcmToRos(lcm_cip.cartesian_damping); + ros_cip.nullspace_damping = lcm_cip.nullspace_damping; + ros_cip.cartesian_stiffness = cvqLcmToRos(lcm_cip.cartesian_stiffness); + ros_cip.nullspace_stiffness = lcm_cip.nullspace_stiffness; + return ros_cip; +} +cartesian_impedance_parameters cartesianImpedanceParamsRosToLcm(const msgs::CartesianImpedanceParameters& ros_cip) { + cartesian_impedance_parameters lcm_cip; + lcm_cip.cartesian_damping = cvqRosToLcm(ros_cip.cartesian_damping); + lcm_cip.nullspace_damping = ros_cip.nullspace_damping; + lcm_cip.cartesian_stiffness = cvqRosToLcm(ros_cip.cartesian_stiffness); + lcm_cip.nullspace_stiffness = ros_cip.nullspace_stiffness; + return lcm_cip; +} - msgs::CartesianControlModeLimits cartesianControlModeLimitsLcmToRos(const cartesian_control_mode_limits& lcm_ccml) - { - msgs::CartesianControlModeLimits ros_ccml; - ros_ccml.max_cartesian_velocity = cvqLcmToRos(lcm_ccml.max_cartesian_velocity); - ros_ccml.max_path_deviation = cvqLcmToRos(lcm_ccml.max_path_deviation); - ros_ccml.max_control_force = cvqLcmToRos(lcm_ccml.max_control_force); - ros_ccml.stop_on_max_control_force = static_cast(lcm_ccml.stop_on_max_control_force); - return ros_ccml; - } +msgs::JointPathExecutionParameters jointPexpLcmToRos(const joint_path_execution_parameters& lcm_pexp) { + msgs::JointPathExecutionParameters ros_pexp; + ros_pexp.joint_relative_acceleration = lcm_pexp.joint_relative_acceleration; + ros_pexp.joint_relative_velocity = lcm_pexp.joint_relative_velocity; + ros_pexp.override_joint_acceleration = lcm_pexp.override_joint_acceleration; + return ros_pexp; +} - cartesian_control_mode_limits cartesianControlModeLimitsRosToLcm(const msgs::CartesianControlModeLimits& ros_ccml) - { - cartesian_control_mode_limits lcm_ccml; - lcm_ccml.max_cartesian_velocity = cvqRosToLcm(ros_ccml.max_cartesian_velocity); - lcm_ccml.max_path_deviation = cvqRosToLcm(ros_ccml.max_path_deviation); - lcm_ccml.max_control_force = cvqRosToLcm(ros_ccml.max_control_force); - lcm_ccml.stop_on_max_control_force = static_cast(ros_ccml.stop_on_max_control_force); - return lcm_ccml; - } +joint_path_execution_parameters jointPexpRosToLcm(const msgs::JointPathExecutionParameters& ros_pexp) { + joint_path_execution_parameters lcm_pexp; + lcm_pexp.joint_relative_acceleration = ros_pexp.joint_relative_acceleration; + lcm_pexp.joint_relative_velocity = ros_pexp.joint_relative_velocity; + lcm_pexp.override_joint_acceleration = ros_pexp.override_joint_acceleration; + return lcm_pexp; +} +msgs::CartesianPathExecutionParameters cartesianPexpLcmToRos(const cartesian_path_execution_parameters& lcm_pexp) { + msgs::CartesianPathExecutionParameters ros_pexp; + ros_pexp.max_velocity = cvqLcmToRos(lcm_pexp.max_velocity); + ros_pexp.max_acceleration = cvqLcmToRos(lcm_pexp.max_acceleration); + ros_pexp.max_nullspace_velocity = lcm_pexp.max_nullspace_velocity; + ros_pexp.max_nullspace_acceleration = lcm_pexp.max_nullspace_acceleration; + return ros_pexp; +} - msgs::ControlMode controlModeLcmToRos(const control_mode& lcm_cm) - { - msgs::ControlMode ros_cm; - ros_cm.mode = (uint8_t)lcm_cm.mode; - return ros_cm; - } +cartesian_path_execution_parameters cartesianPexpRosToLcm(const msgs::CartesianPathExecutionParameters& ros_pexp) { + cartesian_path_execution_parameters lcm_pexp; + lcm_pexp.max_velocity = cvqRosToLcm(ros_pexp.max_velocity); + lcm_pexp.max_acceleration = cvqRosToLcm(ros_pexp.max_acceleration); + lcm_pexp.max_nullspace_velocity = ros_pexp.max_nullspace_velocity; + lcm_pexp.max_nullspace_acceleration = ros_pexp.max_nullspace_acceleration; + return lcm_pexp; +} - control_mode controlModeRosToLcm(const msgs::ControlMode& ros_cm) - { - control_mode lcm_cm; - lcm_cm.mode = (int8_t)ros_cm.mode; - return lcm_cm; - } +msgs::CartesianControlModeLimits cartesianControlModeLimitsLcmToRos(const cartesian_control_mode_limits& lcm_ccml) { + msgs::CartesianControlModeLimits ros_ccml; + ros_ccml.max_cartesian_velocity = cvqLcmToRos(lcm_ccml.max_cartesian_velocity); + ros_ccml.max_path_deviation = cvqLcmToRos(lcm_ccml.max_path_deviation); + ros_ccml.max_control_force = cvqLcmToRos(lcm_ccml.max_control_force); + ros_ccml.stop_on_max_control_force = static_cast(lcm_ccml.stop_on_max_control_force); + return ros_ccml; +} +cartesian_control_mode_limits cartesianControlModeLimitsRosToLcm(const msgs::CartesianControlModeLimits& ros_ccml) { + cartesian_control_mode_limits lcm_ccml; + lcm_ccml.max_cartesian_velocity = cvqRosToLcm(ros_ccml.max_cartesian_velocity); + lcm_ccml.max_path_deviation = cvqRosToLcm(ros_ccml.max_path_deviation); + lcm_ccml.max_control_force = cvqRosToLcm(ros_ccml.max_control_force); + lcm_ccml.stop_on_max_control_force = static_cast(ros_ccml.stop_on_max_control_force); + return lcm_ccml; +} - msgs::MotionStatus motionStatusLcmToRos(const motion_status& lcm_status) - { - msgs::MotionStatus ros_status; - ros_status.commanded_cartesian_pose = poseLcmToRos(lcm_status.commanded_cartesian_pose); - ros_status.commanded_cartesian_pose_abc = cvqLcmToRos(lcm_status.commanded_cartesian_pose_abc); - ros_status.commanded_joint_position = jvqLcmToRos(lcm_status.commanded_joint_position); - ros_status.estimated_external_torque = jvqLcmToRos(lcm_status.estimated_external_torque); - ros_status.estimated_external_wrench = cvqLcmToRos(lcm_status.estimated_external_wrench); - ros_status.measured_cartesian_pose = poseLcmToRos(lcm_status.measured_cartesian_pose); - ros_status.measured_cartesian_pose_abc = cvqLcmToRos(lcm_status.measured_cartesian_pose_abc); - ros_status.measured_joint_position = jvqLcmToRos(lcm_status.measured_joint_position); - ros_status.measured_joint_torque = jvqLcmToRos(lcm_status.measured_joint_torque); - ros_status.measured_joint_velocity = jvqLcmToRos(lcm_status.measured_joint_velocity); - ros_status.active_control_mode = controlModeLcmToRos(lcm_status.active_control_mode); - ros_status.header.stamp = ros::Time(lcm_status.timestamp); - return ros_status; - } +msgs::ControlMode controlModeLcmToRos(const control_mode& lcm_cm) { + msgs::ControlMode ros_cm; + ros_cm.mode = (uint8_t)lcm_cm.mode; + return ros_cm; +} - motion_command motionCommandRosToLcm(const msgs::MotionCommand& ros_command) - { - motion_command lcm_command; - lcm_command.cartesian_pose = poseRosToLcm(ros_command.cartesian_pose); - lcm_command.joint_position = jvqRosToLcm(ros_command.joint_position); - lcm_command.joint_velocity = jvqRosToLcm(ros_command.joint_velocity); - lcm_command.control_mode = controlModeRosToLcm(ros_command.control_mode); - lcm_command.timestamp = ros_command.header.stamp.toSec(); - return lcm_command; - } +control_mode controlModeRosToLcm(const msgs::ControlMode& ros_cm) { + control_mode lcm_cm; + lcm_cm.mode = (int8_t)ros_cm.mode; + return lcm_cm; +} +msgs::MotionStatus motionStatusLcmToRos(const motion_status& lcm_status) { + msgs::MotionStatus ros_status; + ros_status.commanded_cartesian_pose = poseLcmToRos(lcm_status.commanded_cartesian_pose); + ros_status.commanded_cartesian_pose_abc = cvqLcmToRos(lcm_status.commanded_cartesian_pose_abc); + ros_status.commanded_joint_position = jvqLcmToRos(lcm_status.commanded_joint_position); + ros_status.estimated_external_torque = jvqLcmToRos(lcm_status.estimated_external_torque); + ros_status.estimated_external_wrench = cvqLcmToRos(lcm_status.estimated_external_wrench); + ros_status.measured_cartesian_pose = poseLcmToRos(lcm_status.measured_cartesian_pose); + ros_status.measured_cartesian_pose_abc = cvqLcmToRos(lcm_status.measured_cartesian_pose_abc); + ros_status.measured_joint_position = jvqLcmToRos(lcm_status.measured_joint_position); + ros_status.measured_joint_torque = jvqLcmToRos(lcm_status.measured_joint_torque); + ros_status.measured_joint_velocity = jvqLcmToRos(lcm_status.measured_joint_velocity); + ros_status.active_control_mode = controlModeLcmToRos(lcm_status.active_control_mode); + ros_status.header.stamp = ros::Time(lcm_status.timestamp); + return ros_status; +} - msgs::ControlModeParameters controlModeParamsLcmToRos(const control_mode_parameters& lcm_cmp) - { - msgs::ControlModeParameters ros_cmp; - ros_cmp.cartesian_control_mode_limits = cartesianControlModeLimitsLcmToRos(lcm_cmp.cartesian_control_mode_limits); - ros_cmp.cartesian_impedance_params = cartesianImpedanceParamsLcmToRos(lcm_cmp.cartesian_impedance_params); - ros_cmp.joint_impedance_params = jointImpedanceParamsLcmToRos(lcm_cmp.joint_impedance_params); - ros_cmp.joint_path_execution_params = jointPexpLcmToRos(lcm_cmp.joint_path_execution_params); - ros_cmp.cartesian_path_execution_params = cartesianPexpLcmToRos(lcm_cmp.cartesian_path_execution_params); - ros_cmp.control_mode = controlModeLcmToRos(lcm_cmp.control_mode); - ros_cmp.header.stamp = ros::Time(lcm_cmp.timestamp); - return ros_cmp; - } +motion_command motionCommandRosToLcm(const msgs::MotionCommand& ros_command) { + motion_command lcm_command; + lcm_command.cartesian_pose = poseRosToLcm(ros_command.cartesian_pose); + lcm_command.joint_position = jvqRosToLcm(ros_command.joint_position); + lcm_command.joint_velocity = jvqRosToLcm(ros_command.joint_velocity); + lcm_command.control_mode = controlModeRosToLcm(ros_command.control_mode); + lcm_command.timestamp = ros_command.header.stamp.toSec(); + return lcm_command; +} - control_mode_parameters controlModeParamsRosToLcm(const msgs::ControlModeParameters& ros_cmp) - { - control_mode_parameters lcm_cmp; - lcm_cmp.cartesian_control_mode_limits = cartesianControlModeLimitsRosToLcm(ros_cmp.cartesian_control_mode_limits); - lcm_cmp.cartesian_impedance_params = cartesianImpedanceParamsRosToLcm(ros_cmp.cartesian_impedance_params); - lcm_cmp.joint_impedance_params = jointImpedanceParamsRosToLcm(ros_cmp.joint_impedance_params); - lcm_cmp.joint_path_execution_params = jointPexpRosToLcm(ros_cmp.joint_path_execution_params); - lcm_cmp.cartesian_path_execution_params = cartesianPexpRosToLcm(ros_cmp.cartesian_path_execution_params); - lcm_cmp.control_mode = controlModeRosToLcm(ros_cmp.control_mode); - lcm_cmp.timestamp = ros_cmp.header.stamp.toSec(); - return lcm_cmp; - } +msgs::ControlModeParameters controlModeParamsLcmToRos(const control_mode_parameters& lcm_cmp) { + msgs::ControlModeParameters ros_cmp; + ros_cmp.cartesian_control_mode_limits = cartesianControlModeLimitsLcmToRos(lcm_cmp.cartesian_control_mode_limits); + ros_cmp.cartesian_impedance_params = cartesianImpedanceParamsLcmToRos(lcm_cmp.cartesian_impedance_params); + ros_cmp.joint_impedance_params = jointImpedanceParamsLcmToRos(lcm_cmp.joint_impedance_params); + ros_cmp.joint_path_execution_params = jointPexpLcmToRos(lcm_cmp.joint_path_execution_params); + ros_cmp.cartesian_path_execution_params = cartesianPexpLcmToRos(lcm_cmp.cartesian_path_execution_params); + ros_cmp.control_mode = controlModeLcmToRos(lcm_cmp.control_mode); + ros_cmp.header.stamp = ros::Time(lcm_cmp.timestamp); + return ros_cmp; +} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // The class that does the actual communication - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +control_mode_parameters controlModeParamsRosToLcm(const msgs::ControlModeParameters& ros_cmp) { + control_mode_parameters lcm_cmp; + lcm_cmp.cartesian_control_mode_limits = cartesianControlModeLimitsRosToLcm(ros_cmp.cartesian_control_mode_limits); + lcm_cmp.cartesian_impedance_params = cartesianImpedanceParamsRosToLcm(ros_cmp.cartesian_impedance_params); + lcm_cmp.joint_impedance_params = jointImpedanceParamsRosToLcm(ros_cmp.joint_impedance_params); + lcm_cmp.joint_path_execution_params = jointPexpRosToLcm(ros_cmp.joint_path_execution_params); + lcm_cmp.cartesian_path_execution_params = cartesianPexpRosToLcm(ros_cmp.cartesian_path_execution_params); + lcm_cmp.control_mode = controlModeRosToLcm(ros_cmp.control_mode); + lcm_cmp.timestamp = ros_cmp.header.stamp.toSec(); + return lcm_cmp; +} - IIWAHardwareInterface::IIWAHardwareInterface( - const std::shared_ptr& send_lcm_ptr, - const std::shared_ptr& recv_lcm_ptr, - const std::string& motion_command_channel_name, - const std::string& motion_status_channel_name, - const std::function& motion_status_callback_fn, - const std::string& control_mode_command_channel_name, - const std::string& control_mode_status_channel_name, - const std::function& control_mode_status_callback_fn) - : send_lcm_ptr_(send_lcm_ptr), recv_lcm_ptr_(recv_lcm_ptr) - , motion_command_channel_name_(motion_command_channel_name) - , motion_status_channel_name_(motion_status_channel_name) - , motion_status_callback_fn_(motion_status_callback_fn) - , control_mode_command_channel_name_(control_mode_command_channel_name) - , control_mode_status_channel_name_(control_mode_status_channel_name) - , control_mode_status_callback_fn_(control_mode_status_callback_fn) - { - // Check that the LCM objects are ready to communicate before using them - if (send_lcm_ptr_->good() != true) - { - throw std::invalid_argument("Send LCM interface is not good"); - } - if (recv_lcm_ptr_->good() != true) - { - throw std::invalid_argument("Receive LCM interface is not good"); - } - recv_lcm_ptr_->subscribe(motion_status_channel_name_, &IIWAHardwareInterface::InternalMotionStatusLCMCallback, this); - recv_lcm_ptr_->subscribe(control_mode_status_channel_name_, &IIWAHardwareInterface::InternalControlModeStatusLCMCallback, this); - } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// The class that does the actual communication +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +IIWAHardwareInterface::IIWAHardwareInterface( + const std::shared_ptr& send_lcm_ptr, const std::shared_ptr& recv_lcm_ptr, + const std::string& motion_command_channel_name, const std::string& motion_status_channel_name, + const std::function& motion_status_callback_fn, + const std::string& control_mode_command_channel_name, const std::string& control_mode_status_channel_name, + const std::function& control_mode_status_callback_fn) + : send_lcm_ptr_(send_lcm_ptr), + recv_lcm_ptr_(recv_lcm_ptr), + motion_command_channel_name_(motion_command_channel_name), + motion_status_channel_name_(motion_status_channel_name), + motion_status_callback_fn_(motion_status_callback_fn), + control_mode_command_channel_name_(control_mode_command_channel_name), + control_mode_status_channel_name_(control_mode_status_channel_name), + control_mode_status_callback_fn_(control_mode_status_callback_fn) { + // Check that the LCM objects are ready to communicate before using them + if (send_lcm_ptr_->good() != true) { + throw std::invalid_argument("Send LCM interface is not good"); + } + if (recv_lcm_ptr_->good() != true) { + throw std::invalid_argument("Receive LCM interface is not good"); + } + recv_lcm_ptr_->subscribe(motion_status_channel_name_, &IIWAHardwareInterface::InternalMotionStatusLCMCallback, this); + recv_lcm_ptr_->subscribe(control_mode_status_channel_name_, + &IIWAHardwareInterface::InternalControlModeStatusLCMCallback, this); +} - bool IIWAHardwareInterface::SendMotionCommandMessage(const msgs::MotionCommand& command) - { - const motion_command lcm_command = motionCommandRosToLcm(command); - const int ret = send_lcm_ptr_->publish(motion_command_channel_name_, &lcm_command); - if (ret == 0) - { - return true; - } - else - { - return false; - } - } +bool IIWAHardwareInterface::SendMotionCommandMessage(const msgs::MotionCommand& command) { + const motion_command lcm_command = motionCommandRosToLcm(command); + const int ret = send_lcm_ptr_->publish(motion_command_channel_name_, &lcm_command); + if (ret == 0) { + return true; + } else { + return false; + } +} - bool IIWAHardwareInterface::SendControlModeCommandMessage(const msgs::ControlModeParameters& command) - { - const control_mode_parameters lcm_command = controlModeParamsRosToLcm(command); - const int ret = send_lcm_ptr_->publish(control_mode_command_channel_name_, &lcm_command); - if (ret == 0) - { - return true; - } - else - { - return false; - } - } +bool IIWAHardwareInterface::SendControlModeCommandMessage(const msgs::ControlModeParameters& command) { + const control_mode_parameters lcm_command = controlModeParamsRosToLcm(command); + const int ret = send_lcm_ptr_->publish(control_mode_command_channel_name_, &lcm_command); + if (ret == 0) { + return true; + } else { + return false; + } +} - void IIWAHardwareInterface::InternalMotionStatusLCMCallback( - const lcm::ReceiveBuffer* buffer, - const std::string& channel, - const motion_status* status_msg) - { - UNUSED(buffer); - UNUSED(channel); - const msgs::MotionStatus ros_status = motionStatusLcmToRos(*status_msg); - motion_status_callback_fn_(ros_status); - } +void IIWAHardwareInterface::InternalMotionStatusLCMCallback(const lcm::ReceiveBuffer* buffer, + const std::string& channel, + const motion_status* status_msg) { + UNUSED(buffer); + UNUSED(channel); + const msgs::MotionStatus ros_status = motionStatusLcmToRos(*status_msg); + motion_status_callback_fn_(ros_status); +} - void IIWAHardwareInterface::InternalControlModeStatusLCMCallback( - const lcm::ReceiveBuffer* buffer, - const std::string& channel, - const control_mode_parameters* status_msg) - { - UNUSED(buffer); - UNUSED(channel); - const msgs::ControlModeParameters ros_status = controlModeParamsLcmToRos(*status_msg); - control_mode_status_callback_fn_(ros_status); - } +void IIWAHardwareInterface::InternalControlModeStatusLCMCallback(const lcm::ReceiveBuffer* buffer, + const std::string& channel, + const control_mode_parameters* status_msg) { + UNUSED(buffer); + UNUSED(channel); + const msgs::ControlModeParameters ros_status = controlModeParamsLcmToRos(*status_msg); + control_mode_status_callback_fn_(ros_status); } +} // namespace victor_hardware_interface diff --git a/victor_hardware_interface/src/victor_hardware_interface/minimal_arm_wrapper_interface.cpp b/victor_hardware_interface/src/victor_hardware_interface/minimal_arm_wrapper_interface.cpp index 59cfb62a..c5137db0 100644 --- a/victor_hardware_interface/src/victor_hardware_interface/minimal_arm_wrapper_interface.cpp +++ b/victor_hardware_interface/src/victor_hardware_interface/minimal_arm_wrapper_interface.cpp @@ -3,1164 +3,976 @@ // ROS #include +#include + // ROS message headers #include "victor_hardware_interface_msgs/ControlModeParameters.h" +#include "victor_hardware_interface_msgs/GetControlMode.h" #include "victor_hardware_interface_msgs/MotionCommand.h" #include "victor_hardware_interface_msgs/MotionStatus.h" #include "victor_hardware_interface_msgs/Robotiq3FingerCommand.h" #include "victor_hardware_interface_msgs/Robotiq3FingerStatus.h" #include "victor_hardware_interface_msgs/SetControlMode.h" -#include "victor_hardware_interface_msgs/GetControlMode.h" // LCM #include "victor_hardware_interface/iiwa_hardware_interface.hpp" #include "victor_hardware_interface/robotiq_3finger_hardware_interface.hpp" -namespace victor_hardware_interface -{ - namespace msgs = victor_hardware_interface_msgs; - - inline msgs::ControlModeParameters mergeControlModeParameters( - const msgs::ControlModeParameters& active_control_mode, - const msgs::ControlModeParameters& new_control_mode); - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Helpers to test if two messages are equivalent - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - bool jvqEqual(const msgs::JointValueQuantity& jvq1, const msgs::JointValueQuantity& jvq2) - { - if (jvq1.joint_1 != jvq2.joint_1) - { - return false; - } - else if (jvq1.joint_2 != jvq2.joint_2) - { - return false; - } - else if (jvq1.joint_3 != jvq2.joint_3) - { - return false; - } - else if (jvq1.joint_4 != jvq2.joint_4) - { - return false; - } - else if (jvq1.joint_5 != jvq2.joint_5) - { - return false; - } - else if (jvq1.joint_6 != jvq2.joint_6) - { - return false; - } - else if (jvq1.joint_7 != jvq2.joint_7) - { - return false; - } - else - { - return true; - } - } +namespace victor_hardware_interface { +namespace msgs = victor_hardware_interface_msgs; + +inline msgs::ControlModeParameters mergeControlModeParameters(const msgs::ControlModeParameters& active_control_mode, + const msgs::ControlModeParameters& new_control_mode); + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Helpers to test if two messages are equivalent +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool jvqEqual(const msgs::JointValueQuantity& jvq1, const msgs::JointValueQuantity& jvq2) { + if (jvq1.joint_1 != jvq2.joint_1) { + return false; + } else if (jvq1.joint_2 != jvq2.joint_2) { + return false; + } else if (jvq1.joint_3 != jvq2.joint_3) { + return false; + } else if (jvq1.joint_4 != jvq2.joint_4) { + return false; + } else if (jvq1.joint_5 != jvq2.joint_5) { + return false; + } else if (jvq1.joint_6 != jvq2.joint_6) { + return false; + } else if (jvq1.joint_7 != jvq2.joint_7) { + return false; + } else { + return true; + } +} - bool cvqEqual(const msgs::CartesianValueQuantity& cvq1, const msgs::CartesianValueQuantity& cvq2) - { - if (cvq1.x != cvq2.x) - { - return false; - } - else if (cvq1.y != cvq2.y) - { - return false; - } - else if (cvq1.z != cvq2.z) - { - return false; - } - else if (cvq1.a != cvq2.a) - { - return false; - } - else if (cvq1.b != cvq2.b) - { - return false; - } - else if (cvq1.c != cvq2.c) - { - return false; - } - else - { - return true; - } - } +bool cvqEqual(const msgs::CartesianValueQuantity& cvq1, const msgs::CartesianValueQuantity& cvq2) { + if (cvq1.x != cvq2.x) { + return false; + } else if (cvq1.y != cvq2.y) { + return false; + } else if (cvq1.z != cvq2.z) { + return false; + } else if (cvq1.a != cvq2.a) { + return false; + } else if (cvq1.b != cvq2.b) { + return false; + } else if (cvq1.c != cvq2.c) { + return false; + } else { + return true; + } +} - bool jointPexpEqual(const msgs::JointPathExecutionParameters& pexp1, const msgs::JointPathExecutionParameters& pexp2) - { - if (pexp1.joint_relative_acceleration != pexp2.joint_relative_acceleration) - { - return false; - } - else if (pexp1.joint_relative_velocity != pexp2.joint_relative_velocity) - { - return false; - } - else if (pexp1.override_joint_acceleration != pexp2.override_joint_acceleration) - { - return false; - } - else - { - return true; - } - } +bool jointPexpEqual(const msgs::JointPathExecutionParameters& pexp1, const msgs::JointPathExecutionParameters& pexp2) { + if (pexp1.joint_relative_acceleration != pexp2.joint_relative_acceleration) { + return false; + } else if (pexp1.joint_relative_velocity != pexp2.joint_relative_velocity) { + return false; + } else if (pexp1.override_joint_acceleration != pexp2.override_joint_acceleration) { + return false; + } else { + return true; + } +} - bool cartesianPexpEqual(const msgs::CartesianPathExecutionParameters& pexp1, const msgs::CartesianPathExecutionParameters& pexp2) - { - if (cvqEqual(pexp1.max_velocity, pexp2.max_velocity) == false) - { - return false; - } - else if (cvqEqual(pexp1.max_acceleration, pexp2.max_acceleration) == false) - { - return false; - } - else if (pexp1.max_nullspace_velocity != pexp2.max_nullspace_velocity) - { - return false; - } - else if (pexp1.max_nullspace_acceleration != pexp2.max_nullspace_acceleration) - { - return false; - } - else - { - return true; - } - } +bool cartesianPexpEqual(const msgs::CartesianPathExecutionParameters& pexp1, + const msgs::CartesianPathExecutionParameters& pexp2) { + if (!cvqEqual(pexp1.max_velocity, pexp2.max_velocity)) { + return false; + } else if (!cvqEqual(pexp1.max_acceleration, pexp2.max_acceleration)) { + return false; + } else if (pexp1.max_nullspace_velocity != pexp2.max_nullspace_velocity) { + return false; + } else if (pexp1.max_nullspace_acceleration != pexp2.max_nullspace_acceleration) { + return false; + } else { + return true; + } +} - bool controlModeParamsEqual(const msgs::ControlModeParameters& params1, const msgs::ControlModeParameters& params2) - { - // Control mode parameter - const bool cm_equal = (params1.control_mode.mode == params2.control_mode.mode); - - // Path Execuition mode parameters - const bool jpexp_equal = jointPexpEqual(params1.joint_path_execution_params, params2.joint_path_execution_params); - const bool cpexp_equal = cartesianPexpEqual(params1.cartesian_path_execution_params, params2.cartesian_path_execution_params); - - // Joint Impedance mode parameters - const bool jd_equal = jvqEqual(params1.joint_impedance_params.joint_damping, params2.joint_impedance_params.joint_damping); - const bool js_equal = jvqEqual(params1.joint_impedance_params.joint_stiffness, params2.joint_impedance_params.joint_stiffness); - - // Cartesian Impedance mode parameters - const bool cd_equal = cvqEqual(params1.cartesian_impedance_params.cartesian_damping, params2.cartesian_impedance_params.cartesian_damping); - const bool nd_equal = (params1.cartesian_impedance_params.nullspace_damping == params2.cartesian_impedance_params.nullspace_damping); - const bool cs_equal = cvqEqual(params1.cartesian_impedance_params.cartesian_stiffness, params2.cartesian_impedance_params.cartesian_stiffness); - const bool ns_equal = (params1.cartesian_impedance_params.nullspace_stiffness == params2.cartesian_impedance_params.nullspace_stiffness); - - // Cartesian control mode limits parameters - const bool mpd_equal = cvqEqual(params1.cartesian_control_mode_limits.max_path_deviation, params2.cartesian_control_mode_limits.max_path_deviation); - const bool mcv_equal = cvqEqual(params1.cartesian_control_mode_limits.max_cartesian_velocity, params2.cartesian_control_mode_limits.max_cartesian_velocity); - const bool mcf_equal = cvqEqual(params1.cartesian_control_mode_limits.max_control_force, params2.cartesian_control_mode_limits.max_control_force); - const bool smcf_equal = (params1.cartesian_control_mode_limits.stop_on_max_control_force == params2.cartesian_control_mode_limits.stop_on_max_control_force); - - if (cm_equal && - jpexp_equal && cpexp_equal && - jd_equal && js_equal && - cd_equal && nd_equal && cs_equal && ns_equal && - mpd_equal && mcv_equal && mcf_equal && smcf_equal) - { - return true; - } - else - { - return false; - } - } +bool controlModeParamsEqual(const msgs::ControlModeParameters& params1, const msgs::ControlModeParameters& params2) { + // Control mode parameter + const bool cm_equal = (params1.control_mode.mode == params2.control_mode.mode); + + // Path Execuition mode parameters + const bool jpexp_equal = jointPexpEqual(params1.joint_path_execution_params, params2.joint_path_execution_params); + const bool cpexp_equal = + cartesianPexpEqual(params1.cartesian_path_execution_params, params2.cartesian_path_execution_params); + + // Joint Impedance mode parameters + const bool jd_equal = + jvqEqual(params1.joint_impedance_params.joint_damping, params2.joint_impedance_params.joint_damping); + const bool js_equal = + jvqEqual(params1.joint_impedance_params.joint_stiffness, params2.joint_impedance_params.joint_stiffness); + + // Cartesian Impedance mode parameters + const bool cd_equal = cvqEqual(params1.cartesian_impedance_params.cartesian_damping, + params2.cartesian_impedance_params.cartesian_damping); + const bool nd_equal = + (params1.cartesian_impedance_params.nullspace_damping == params2.cartesian_impedance_params.nullspace_damping); + const bool cs_equal = cvqEqual(params1.cartesian_impedance_params.cartesian_stiffness, + params2.cartesian_impedance_params.cartesian_stiffness); + const bool ns_equal = (params1.cartesian_impedance_params.nullspace_stiffness == + params2.cartesian_impedance_params.nullspace_stiffness); + + // Cartesian control mode limits parameters + const bool mpd_equal = cvqEqual(params1.cartesian_control_mode_limits.max_path_deviation, + params2.cartesian_control_mode_limits.max_path_deviation); + const bool mcv_equal = cvqEqual(params1.cartesian_control_mode_limits.max_cartesian_velocity, + params2.cartesian_control_mode_limits.max_cartesian_velocity); + const bool mcf_equal = cvqEqual(params1.cartesian_control_mode_limits.max_control_force, + params2.cartesian_control_mode_limits.max_control_force); + const bool smcf_equal = (params1.cartesian_control_mode_limits.stop_on_max_control_force == + params2.cartesian_control_mode_limits.stop_on_max_control_force); + + if (cm_equal && jpexp_equal && cpexp_equal && jd_equal && js_equal && cd_equal && nd_equal && cs_equal && ns_equal && + mpd_equal && mcv_equal && mcf_equal && smcf_equal) { + return true; + } else { + return false; + } +} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Constructor and single function called externally - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - MinimalArmWrapperInterface::MinimalArmWrapperInterface( - ros::NodeHandle& nh, - const std::shared_ptr& send_lcm_ptr, - const std::shared_ptr& recv_lcm_ptr, - const std::string& cartesian_control_frame, - const double set_control_mode_timeout, - // ROS Topics - const std::string& motion_command_topic, - const std::string& motion_status_topic, - const std::string& control_mode_status_topic, - const std::string& get_control_mode_service, - const std::string& set_control_mode_service, - const std::string& gripper_command_topic, - const std::string& gripper_status_topic, - // LCM channels - const std::string& motion_command_channel, - const std::string& motion_status_channel, - const std::string& control_mode_command_channel, - const std::string& control_mode_status_channel, - const std::string& gripper_command_channel, - const std::string& gripper_status_channel) - : nh_(nh) - , cartesian_control_frame_(cartesian_control_frame) - , set_control_mode_timeout_(set_control_mode_timeout) - , send_lcm_ptr_(send_lcm_ptr) - , recv_lcm_ptr_(recv_lcm_ptr) - { - // Verify that the control frame is a valid ROS TF name (or at least not empty) - // TODO: Are there any other limits on TF frame names other than "non-empty"? see Issue #28 - if (cartesian_control_frame_ == "") - { - throw std::invalid_argument("Cartesian control frame [""] is not valid"); - } - - // Set up IIWA LCM interface - const auto motion_status_callback_fn = [&] (const msgs::MotionStatus& motion_status) - { - return motionStatusLCMCallback(motion_status); - }; - const auto control_mode_status_callback_fn = [&] (const msgs::ControlModeParameters& control_mode_status) - { - return controlModeStatusLCMCallback(control_mode_status); - }; - iiwa_ptr_ = std::unique_ptr(new IIWAHardwareInterface( - send_lcm_ptr_, recv_lcm_ptr_, - motion_command_channel, motion_status_channel, motion_status_callback_fn, - control_mode_command_channel, control_mode_status_channel, control_mode_status_callback_fn)); - - // Set up Robotiq LCM interface - const auto gripper_status_callback_fn = [&] (const msgs::Robotiq3FingerStatus& gripper_status) - { - return gripperStatusLCMCallback(gripper_status); - }; - robotiq_ptr_ = std::unique_ptr( - new Robotiq3FingerHardwareInterface( - send_lcm_ptr_, recv_lcm_ptr_, - gripper_command_channel, gripper_status_channel, gripper_status_callback_fn)); - - // Set up ROS interfaces - nh_.setCallbackQueue(&ros_callback_queue_); - motion_status_pub_ = nh_.advertise(motion_status_topic, 1, false); - control_mode_status_pub_ = nh_.advertise(control_mode_status_topic, 1, false); - gripper_status_pub_ = nh_.advertise(gripper_status_topic, 1, false); - motion_command_sub_ = nh_.subscribe(motion_command_topic, 1, &MinimalArmWrapperInterface::motionCommandROSCallback, this); - gripper_command_sub_ = nh_.subscribe(gripper_command_topic, 1, &MinimalArmWrapperInterface::gripperCommandROSCallback, this);; - set_control_mode_server_ = nh_.advertiseService(set_control_mode_service, &MinimalArmWrapperInterface::setControlModeCallback, this); - get_control_mode_server_ = nh_.advertiseService(get_control_mode_service, &MinimalArmWrapperInterface::getControlModeCallback, this); - } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Constructor and single function called externally +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +MinimalArmWrapperInterface::MinimalArmWrapperInterface( + ros::NodeHandle& nh, std::shared_ptr send_lcm_ptr, std::shared_ptr recv_lcm_ptr, + std::string cartesian_control_frame, const double set_control_mode_timeout, + // ROS Topics + const std::string& motion_command_topic, const std::string& motion_status_topic, + const std::string& control_mode_status_topic, const std::string& get_control_mode_service, + const std::string& set_control_mode_service, const std::string& gripper_command_topic, + const std::string& gripper_status_topic, + // LCM channels + const std::string& motion_command_channel, const std::string& motion_status_channel, + const std::string& control_mode_command_channel, const std::string& control_mode_status_channel, + const std::string& gripper_command_channel, const std::string& gripper_status_channel) + : nh_(nh), + cartesian_control_frame_(std::move(cartesian_control_frame)), + set_control_mode_timeout_(set_control_mode_timeout), + send_lcm_ptr_(std::move(send_lcm_ptr)), + recv_lcm_ptr_(std::move(recv_lcm_ptr)) { + // Verify that the control frame is a valid ROS TF name (or at least not empty) + // TODO: Are there any other limits on TF frame names other than "non-empty"? see Issue #28 + if (cartesian_control_frame_.empty()) { + throw std::invalid_argument( + "Cartesian control frame [" + "] is not valid"); + } + + // Set up IIWA LCM interface + const auto motion_status_callback_fn = [&](const msgs::MotionStatus& motion_status) { + return motionStatusLCMCallback(motion_status); + }; + const auto control_mode_status_callback_fn = [&](const msgs::ControlModeParameters& control_mode_status) { + return controlModeStatusLCMCallback(control_mode_status); + }; + iiwa_ptr_ = std::unique_ptr(new IIWAHardwareInterface( + send_lcm_ptr_, recv_lcm_ptr_, motion_command_channel, motion_status_channel, motion_status_callback_fn, + control_mode_command_channel, control_mode_status_channel, control_mode_status_callback_fn)); + + // Set up Robotiq LCM interface + const auto gripper_status_callback_fn = [&](const msgs::Robotiq3FingerStatus& gripper_status) { + return gripperStatusLCMCallback(gripper_status); + }; + robotiq_ptr_ = std::unique_ptr(new Robotiq3FingerHardwareInterface( + send_lcm_ptr_, recv_lcm_ptr_, gripper_command_channel, gripper_status_channel, gripper_status_callback_fn)); + + // Set up ROS interfaces + nh_.setCallbackQueue(&ros_callback_queue_); + motion_status_pub_ = nh_.advertise(motion_status_topic, 1, false); + control_mode_status_pub_ = nh_.advertise(control_mode_status_topic, 1, false); + gripper_status_pub_ = nh_.advertise(gripper_status_topic, 1, false); + motion_command_sub_ = + nh_.subscribe(motion_command_topic, 1, &MinimalArmWrapperInterface::motionCommandROSCallback, this); + gripper_command_sub_ = + nh_.subscribe(gripper_command_topic, 1, &MinimalArmWrapperInterface::gripperCommandROSCallback, this); + set_control_mode_server_ = + nh_.advertiseService(set_control_mode_service, &MinimalArmWrapperInterface::setControlModeCallback, this); + get_control_mode_server_ = + nh_.advertiseService(get_control_mode_service, &MinimalArmWrapperInterface::getControlModeCallback, this); +} - // The one function called externally - void MinimalArmWrapperInterface::spin() - { - // Start ROS thread - this must happen *after* the LCM objects have been initialized as they use iiwa_ptr_ and - // robotiq_ptr_, so we do so here instead of in the constructor - ROS_INFO_NAMED(ros::this_node::getName(), "Starting ROS spin loop."); - auto ros_callback_thread = std::thread(std::bind(&MinimalArmWrapperInterface::rosSpinThread, this)); - - ROS_INFO_NAMED(ros::this_node::getName(), "Starting LCM spin loop."); - bool lcm_ok = true; - // Continue to loop so long as both ROS and LCM have no un-recoverable errors or SIGINT style interruptions - while (ros::ok() && lcm_ok) - { - const int ret = recv_lcm_ptr_->handleTimeout(LCM_HANDLE_TIMEOUT); - if (ret < 0) - { - lcm_ok = false; - ROS_ERROR_STREAM_NAMED(ros::this_node::getName(), "LCM error: " << ret); - } - } - - ros::shutdown(); - ros_callback_thread.join(); - } +// The one function called externally +void MinimalArmWrapperInterface::spin() { + // Start ROS thread - this must happen *after* the LCM objects have been initialized as they use iiwa_ptr_ and + // robotiq_ptr_, so we do so here instead of in the constructor + ROS_INFO_NAMED(ros::this_node::getName(), "Starting ROS spin loop."); + auto ros_callback_thread = std::thread(std::bind(&MinimalArmWrapperInterface::rosSpinThread, this)); + + ROS_INFO_NAMED(ros::this_node::getName(), "Starting LCM spin loop."); + bool lcm_ok = true; + // Continue to loop so long as both ROS and LCM have no un-recoverable errors or SIGINT style interruptions + while (ros::ok() && lcm_ok) { + const int ret = recv_lcm_ptr_->handleTimeout(LCM_HANDLE_TIMEOUT); + if (ret < 0) { + lcm_ok = false; + ROS_ERROR_STREAM_NAMED(ros::this_node::getName(), "LCM error: " << ret); + } + } + + ros::shutdown(); + ros_callback_thread.join(); +} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - // Internal helper function used to process ROS callbacks - void MinimalArmWrapperInterface::rosSpinThread() - { - const ros::WallDuration timeout(ROS_SPIN_PERIOD); - while (nh_.ok()) - { - ros_callback_queue_.callAvailable(timeout); - } - } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Get/Set Control Mode functionality - //////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - //// Static helpers to check parameters for being in valid ranges for the control mode ///////////////////////// - - std::pair MinimalArmWrapperInterface::validateJointPathExecutionParams( - const msgs::JointPathExecutionParameters& params) - { - bool valid = true; - std::string message; - if (params.joint_relative_velocity <= 0.0 || params.joint_relative_velocity > 1.0) - { - valid = false; - message += "+Invalid joint relative velocity"; - } - if (params.joint_relative_acceleration <= 0.0 || params.joint_relative_acceleration > 1.0) - { - valid = false; - message += "+Invalid joint relative acceleration"; - } - if (params.override_joint_acceleration < 0.0 || params.override_joint_acceleration > 10.0) - { - valid = false; - message += "+Invalid override joint acceleration"; - } - return std::make_pair(valid, message); - } +// Internal helper function used to process ROS callbacks +void MinimalArmWrapperInterface::rosSpinThread() { + const ros::WallDuration timeout(ROS_SPIN_PERIOD); + while (nh_.ok()) { + ros_callback_queue_.callAvailable(timeout); + } +} - std::pair MinimalArmWrapperInterface::validateCartesianPathExecutionParams( - const msgs::CartesianPathExecutionParameters& params) - { - bool valid = true; - std::string message; - - // Velocities, mm/s, rad/s, and 1/s respectively - if (params.max_velocity.x <= 0.0) - { - valid = false; - message += "+Invalid DoF X max velocity"; - } - if (params.max_velocity.y <= 0.0) - { - valid = false; - message += "+Invalid DoF Y max velocity"; - } - if (params.max_velocity.z <= 0.0) - { - valid = false; - message += "+Invalid DoF Z max velocity"; - } - if (params.max_velocity.a <= 0.0) - { - valid = false; - message += "+Invalid DoF A max velocity"; - } - if (params.max_velocity.b <= 0.0) - { - valid = false; - message += "+Invalid DoF B max velocity"; - } - if (params.max_velocity.c <= 0.0) - { - valid = false; - message += "+Invalid DoF C max velocity"; - } - if (params.max_nullspace_velocity <= 0.0) - { - valid = false; - message += "+Invalid nullspace max velocity"; - } - - // Accelerations, mm/s^2, rad/s^2, and 1/s^2 respectively - if (params.max_acceleration.x <= 0.0) - { - valid = false; - message += "+Invalid DoF X max acceleration"; - } - if (params.max_acceleration.y <= 0.0) - { - valid = false; - message += "+Invalid DoF Y max acceleration"; - } - if (params.max_acceleration.z <= 0.0) - { - valid = false; - message += "+Invalid DoF Z max acceleration"; - } - if (params.max_acceleration.a <= 0.0) - { - valid = false; - message += "+Invalid DoF A max acceleration"; - } - if (params.max_acceleration.b <= 0.0) - { - valid = false; - message += "+Invalid DoF B max acceleration"; - } - if (params.max_acceleration.c <= 0.0) - { - valid = false; - message += "+Invalid DoF C max acceleration"; - } - if (params.max_nullspace_acceleration <= 0.0) - { - valid = false; - message += "+Invalid nullspace max acceleration"; - } - - return std::make_pair(valid, message); - } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Get/Set Control Mode functionality +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +//// Static helpers to check parameters for being in valid ranges for the control mode ///////////////////////// + +std::pair MinimalArmWrapperInterface::validateJointPathExecutionParams( + const msgs::JointPathExecutionParameters& params) { + bool valid = true; + std::string message; + if (params.joint_relative_velocity <= 0.0 || params.joint_relative_velocity > 1.0) { + valid = false; + message += "+Invalid joint relative velocity"; + } + if (params.joint_relative_acceleration <= 0.0 || params.joint_relative_acceleration > 1.0) { + valid = false; + message += "+Invalid joint relative acceleration"; + } + if (params.override_joint_acceleration < 0.0 || params.override_joint_acceleration > 10.0) { + valid = false; + message += "+Invalid override joint acceleration"; + } + return std::make_pair(valid, message); +} - std::pair MinimalArmWrapperInterface::validateJointImpedanceParams( - const msgs::JointImpedanceParameters& params) - { - bool valid = true; - std::string message; - - // Joint damping - unitless - if (params.joint_damping.joint_1 < 0.0 || params.joint_damping.joint_1 > 1.0) - { - valid = false; - message += "+Invalid joint 1 damping"; - } - if (params.joint_damping.joint_2 < 0.0 || params.joint_damping.joint_2 > 1.0) - { - valid = false; - message += "+Invalid joint 2 damping"; - } - if (params.joint_damping.joint_3 < 0.0 || params.joint_damping.joint_3 > 1.0) - { - valid = false; - message += "+Invalid joint 3 damping"; - } - if (params.joint_damping.joint_4 < 0.0 || params.joint_damping.joint_4 > 1.0) - { - valid = false; - message += "+Invalid joint 4 damping"; - } - if (params.joint_damping.joint_5 < 0.0 || params.joint_damping.joint_5 > 1.0) - { - valid = false; - message += "+Invalid joint 5 damping"; - } - if (params.joint_damping.joint_6 < 0.0 || params.joint_damping.joint_6 > 1.0) - { - valid = false; - message += "+Invalid joint 6 damping"; - } - if (params.joint_damping.joint_7 < 0.0 || params.joint_damping.joint_7 > 1.0) - { - valid = false; - message += "+Invalid joint 7 damping"; - } - - // Joint stiffness - Nm/rad - if (params.joint_stiffness.joint_1 < 0.0) - { - valid = false; - message += "+Invalid joint 1 stiffness"; - } - if (params.joint_stiffness.joint_2 < 0.0) - { - valid = false; - message += "+Invalid joint 2 stiffness"; - } - if (params.joint_stiffness.joint_3 < 0.0) - { - valid = false; - message += "+Invalid joint 3 stiffness"; - } - if (params.joint_stiffness.joint_4 < 0.0) - { - valid = false; - message += "+Invalid joint 4 stiffness"; - } - if (params.joint_stiffness.joint_5 < 0.0) - { - valid = false; - message += "+Invalid joint 5 stiffness"; - } - if (params.joint_stiffness.joint_6 < 0.0) - { - valid = false; - message += "+Invalid joint 6 stiffness"; - } - if (params.joint_stiffness.joint_7 < 0.0) - { - valid = false; - message += "+Invalid joint 7 stiffness"; - } - - return std::make_pair(valid, message); - } +std::pair MinimalArmWrapperInterface::validateCartesianPathExecutionParams( + const msgs::CartesianPathExecutionParameters& params) { + bool valid = true; + std::string message; + + // Velocities, mm/s, rad/s, and 1/s respectively + if (params.max_velocity.x <= 0.0) { + valid = false; + message += "+Invalid DoF X max velocity"; + } + if (params.max_velocity.y <= 0.0) { + valid = false; + message += "+Invalid DoF Y max velocity"; + } + if (params.max_velocity.z <= 0.0) { + valid = false; + message += "+Invalid DoF Z max velocity"; + } + if (params.max_velocity.a <= 0.0) { + valid = false; + message += "+Invalid DoF A max velocity"; + } + if (params.max_velocity.b <= 0.0) { + valid = false; + message += "+Invalid DoF B max velocity"; + } + if (params.max_velocity.c <= 0.0) { + valid = false; + message += "+Invalid DoF C max velocity"; + } + if (params.max_nullspace_velocity <= 0.0) { + valid = false; + message += "+Invalid nullspace max velocity"; + } + + // Accelerations, mm/s^2, rad/s^2, and 1/s^2 respectively + if (params.max_acceleration.x <= 0.0) { + valid = false; + message += "+Invalid DoF X max acceleration"; + } + if (params.max_acceleration.y <= 0.0) { + valid = false; + message += "+Invalid DoF Y max acceleration"; + } + if (params.max_acceleration.z <= 0.0) { + valid = false; + message += "+Invalid DoF Z max acceleration"; + } + if (params.max_acceleration.a <= 0.0) { + valid = false; + message += "+Invalid DoF A max acceleration"; + } + if (params.max_acceleration.b <= 0.0) { + valid = false; + message += "+Invalid DoF B max acceleration"; + } + if (params.max_acceleration.c <= 0.0) { + valid = false; + message += "+Invalid DoF C max acceleration"; + } + if (params.max_nullspace_acceleration <= 0.0) { + valid = false; + message += "+Invalid nullspace max acceleration"; + } + + return std::make_pair(valid, message); +} - std::pair MinimalArmWrapperInterface::validateCartesianImpedanceParams( - const msgs::CartesianImpedanceParameters& params) - { - bool valid = true; - std::string message; - - // Damping - unitless - if (params.cartesian_damping.x < 0.1 || params.cartesian_damping.x > 1.0) - { - valid = false; - message += "+Invalid DoF X damping"; - } - if (params.cartesian_damping.y < 0.1 || params.cartesian_damping.y > 1.0) - { - valid = false; - message += "+Invalid DoF Y damping"; - } - if (params.cartesian_damping.z < 0.1 || params.cartesian_damping.z > 1.0) - { - valid = false; - message += "+Invalid DoF Z damping"; - } - if (params.cartesian_damping.a < 0.1 || params.cartesian_damping.a > 1.0) - { - valid = false; - message += "+Invalid DoF A damping"; - } - if (params.cartesian_damping.b < 0.1 || params.cartesian_damping.b > 1.0) - { - valid = false; - message += "+Invalid DoF B damping"; - } - if (params.cartesian_damping.c < 0.1 || params.cartesian_damping.c > 1.0) - { - valid = false; - message += "+Invalid DoF C damping"; - } - if (params.nullspace_damping < 0.3 || params.nullspace_damping > 1.0) - { - valid = false; - message += "+Invalid nullspace damping"; - } - - // Stiffness - units N/m, Nm/rad, no idea for nullspace - if (params.cartesian_stiffness.x < 0.0 || params.cartesian_stiffness.x > 5000.0) - { - valid = false; - message += "+Invalid DoF X stiffness"; - } - if (params.cartesian_stiffness.y < 0.0 || params.cartesian_stiffness.y > 5000.0) - { - valid = false; - message += "+Invalid DoF Y stiffness"; - } - if (params.cartesian_stiffness.z < 0.0 || params.cartesian_stiffness.z > 5000.0) - { - valid = false; - message += "+Invalid DoF Z stiffness"; - } - // TODO: original values set by Calder were < 0.1 and > 300.0 - documentation states < 0.0 and > 300.0; why was it set to 0.1? - if (params.cartesian_stiffness.a < 0.0 || params.cartesian_stiffness.a > 300.0) - { - valid = false; - message += "+Invalid DoF A stiffness"; - } - if (params.cartesian_stiffness.b < 0.0 || params.cartesian_stiffness.b > 300.0) - { - valid = false; - message += "+Invalid DoF B stiffness"; - } - if (params.cartesian_stiffness.c < 0.0 || params.cartesian_stiffness.c > 300.0) - { - valid = false; - message += "+Invalid DoF C stiffness"; - } - if (params.nullspace_stiffness < 0.0) - { - valid = false; - message += "+Invalid nullspace stiffness"; - } - - return std::make_pair(valid, message); - } +std::pair MinimalArmWrapperInterface::validateJointImpedanceParams( + const msgs::JointImpedanceParameters& params) { + bool valid = true; + std::string message; + + // Joint damping - unitless + if (params.joint_damping.joint_1 < 0.0 || params.joint_damping.joint_1 > 1.0) { + valid = false; + message += "+Invalid joint 1 damping"; + } + if (params.joint_damping.joint_2 < 0.0 || params.joint_damping.joint_2 > 1.0) { + valid = false; + message += "+Invalid joint 2 damping"; + } + if (params.joint_damping.joint_3 < 0.0 || params.joint_damping.joint_3 > 1.0) { + valid = false; + message += "+Invalid joint 3 damping"; + } + if (params.joint_damping.joint_4 < 0.0 || params.joint_damping.joint_4 > 1.0) { + valid = false; + message += "+Invalid joint 4 damping"; + } + if (params.joint_damping.joint_5 < 0.0 || params.joint_damping.joint_5 > 1.0) { + valid = false; + message += "+Invalid joint 5 damping"; + } + if (params.joint_damping.joint_6 < 0.0 || params.joint_damping.joint_6 > 1.0) { + valid = false; + message += "+Invalid joint 6 damping"; + } + if (params.joint_damping.joint_7 < 0.0 || params.joint_damping.joint_7 > 1.0) { + valid = false; + message += "+Invalid joint 7 damping"; + } + + // Joint stiffness - Nm/rad + if (params.joint_stiffness.joint_1 < 0.0) { + valid = false; + message += "+Invalid joint 1 stiffness"; + } + if (params.joint_stiffness.joint_2 < 0.0) { + valid = false; + message += "+Invalid joint 2 stiffness"; + } + if (params.joint_stiffness.joint_3 < 0.0) { + valid = false; + message += "+Invalid joint 3 stiffness"; + } + if (params.joint_stiffness.joint_4 < 0.0) { + valid = false; + message += "+Invalid joint 4 stiffness"; + } + if (params.joint_stiffness.joint_5 < 0.0) { + valid = false; + message += "+Invalid joint 5 stiffness"; + } + if (params.joint_stiffness.joint_6 < 0.0) { + valid = false; + message += "+Invalid joint 6 stiffness"; + } + if (params.joint_stiffness.joint_7 < 0.0) { + valid = false; + message += "+Invalid joint 7 stiffness"; + } + + return std::make_pair(valid, message); +} - std::pair MinimalArmWrapperInterface::validateCartesianControlModeLimits( - const msgs::CartesianControlModeLimits& params) - { - bool valid = true; - std::string message; - - // Path deviation - if (params.max_path_deviation.x <= 0.0) - { - valid = false; - message += "+Invalid DoF X max path deviation"; - } - if (params.max_path_deviation.y <= 0.0) - { - valid = false; - message += "+Invalid DoF Y max path deviation"; - } - if (params.max_path_deviation.z <= 0.0) - { - valid = false; - message += "+Invalid DoF Z max path deviation"; - } - if (params.max_path_deviation.a <= 0.0) - { - valid = false; - message += "+Invalid DoF A max path deviation"; - } - if (params.max_path_deviation.b <= 0.0) - { - valid = false; - message += "+Invalid DoF B max path deviation"; - } - if (params.max_path_deviation.c <= 0.0) - { - valid = false; - message += "+Invalid DoF C max path deviation"; - } - - // Cartesian velocity - if (params.max_cartesian_velocity.x <= 0.0) - { - valid = false; - message += "+Invalid DoF X max cartesian velocity"; - } - if (params.max_cartesian_velocity.y <= 0.0) - { - valid = false; - message += "+Invalid DoF Y max cartesian velocity"; - } - if (params.max_cartesian_velocity.z <= 0.0) - { - valid = false; - message += "+Invalid DoF Z max cartesian velocity"; - } - if (params.max_cartesian_velocity.a <= 0.0) - { - valid = false; - message += "+Invalid DoF A max cartesian velocity"; - } - if (params.max_cartesian_velocity.b <= 0.0) - { - valid = false; - message += "+Invalid DoF B max cartesian velocity"; - } - if (params.max_cartesian_velocity.c <= 0.0) - { - valid = false; - message += "+Invalid DoF C max cartesian velocity"; - } - - // Cartesian force - if (params.max_control_force.x <= 0.0) - { - valid = false; - message += "+Invalid DoF X max control force"; - } - if (params.max_control_force.y <= 0.0) - { - valid = false; - message += "+Invalid DoF Y max control force"; - } - if (params.max_control_force.z <= 0.0) - { - valid = false; - message += "+Invalid DoF Z max control force"; - } - if (params.max_control_force.a <= 0.0) - { - valid = false; - message += "+Invalid DoF A max control force"; - } - if (params.max_control_force.b <= 0.0) - { - valid = false; - message += "+Invalid DoF B max control force"; - } - if (params.max_control_force.c <= 0.0) - { - valid = false; - message += "+Invalid DoF C max control force"; - } - - return std::make_pair(valid, message); - } +std::pair MinimalArmWrapperInterface::validateCartesianImpedanceParams( + const msgs::CartesianImpedanceParameters& params) { + bool valid = true; + std::string message; + + // Damping - unitless + if (params.cartesian_damping.x < 0.1 || params.cartesian_damping.x > 1.0) { + valid = false; + message += "+Invalid DoF X damping"; + } + if (params.cartesian_damping.y < 0.1 || params.cartesian_damping.y > 1.0) { + valid = false; + message += "+Invalid DoF Y damping"; + } + if (params.cartesian_damping.z < 0.1 || params.cartesian_damping.z > 1.0) { + valid = false; + message += "+Invalid DoF Z damping"; + } + if (params.cartesian_damping.a < 0.1 || params.cartesian_damping.a > 1.0) { + valid = false; + message += "+Invalid DoF A damping"; + } + if (params.cartesian_damping.b < 0.1 || params.cartesian_damping.b > 1.0) { + valid = false; + message += "+Invalid DoF B damping"; + } + if (params.cartesian_damping.c < 0.1 || params.cartesian_damping.c > 1.0) { + valid = false; + message += "+Invalid DoF C damping"; + } + if (params.nullspace_damping < 0.3 || params.nullspace_damping > 1.0) { + valid = false; + message += "+Invalid nullspace damping"; + } + + // Stiffness - units N/m, Nm/rad, no idea for nullspace + if (params.cartesian_stiffness.x < 0.0 || params.cartesian_stiffness.x > 5000.0) { + valid = false; + message += "+Invalid DoF X stiffness"; + } + if (params.cartesian_stiffness.y < 0.0 || params.cartesian_stiffness.y > 5000.0) { + valid = false; + message += "+Invalid DoF Y stiffness"; + } + if (params.cartesian_stiffness.z < 0.0 || params.cartesian_stiffness.z > 5000.0) { + valid = false; + message += "+Invalid DoF Z stiffness"; + } + // TODO: original values set by Calder were < 0.1 and > 300.0 - documentation states < 0.0 and > 300.0; why was it set + // to 0.1? + if (params.cartesian_stiffness.a < 0.0 || params.cartesian_stiffness.a > 300.0) { + valid = false; + message += "+Invalid DoF A stiffness"; + } + if (params.cartesian_stiffness.b < 0.0 || params.cartesian_stiffness.b > 300.0) { + valid = false; + message += "+Invalid DoF B stiffness"; + } + if (params.cartesian_stiffness.c < 0.0 || params.cartesian_stiffness.c > 300.0) { + valid = false; + message += "+Invalid DoF C stiffness"; + } + if (params.nullspace_stiffness < 0.0) { + valid = false; + message += "+Invalid nullspace stiffness"; + } + + return std::make_pair(valid, message); +} - std::pair MinimalArmWrapperInterface::validateControlMode(const msgs::ControlModeParameters& params) - { - bool valid = true; - std::string message; - - // Check the control mode itself - if (params.control_mode.mode != msgs::ControlMode::JOINT_POSITION && - params.control_mode.mode != msgs::ControlMode::JOINT_IMPEDANCE && - params.control_mode.mode != msgs::ControlMode::CARTESIAN_POSE && - params.control_mode.mode != msgs::ControlMode::CARTESIAN_IMPEDANCE) - { - valid = false; - message += "+Invalid control mode"; - } - - // Check each part of the control mode - const auto valid_joint_impedance_params = validateJointImpedanceParams(params.joint_impedance_params); - const auto valid_cartesian_impedance_params = validateCartesianImpedanceParams(params.cartesian_impedance_params); - const auto valid_cartesian_control_mode_limits = validateCartesianControlModeLimits(params.cartesian_control_mode_limits); - const auto valid_joint_path_execution_params = validateJointPathExecutionParams(params.joint_path_execution_params); - const auto valid_cartesian_path_execution_params = validateCartesianPathExecutionParams(params.cartesian_path_execution_params); - - // Aggregate the results - valid &= valid_joint_impedance_params.first && - valid_cartesian_impedance_params.first && - valid_cartesian_control_mode_limits.first && - valid_joint_path_execution_params.first && - valid_cartesian_path_execution_params.first; - - message += valid_joint_impedance_params.second + - valid_cartesian_impedance_params.second + - valid_cartesian_control_mode_limits.second + - valid_joint_path_execution_params.second + - valid_cartesian_path_execution_params.second; - - return std::make_pair(valid, message); - } +std::pair MinimalArmWrapperInterface::validateCartesianControlModeLimits( + const msgs::CartesianControlModeLimits& params) { + bool valid = true; + std::string message; + + // Path deviation + if (params.max_path_deviation.x <= 0.0) { + valid = false; + message += "+Invalid DoF X max path deviation"; + } + if (params.max_path_deviation.y <= 0.0) { + valid = false; + message += "+Invalid DoF Y max path deviation"; + } + if (params.max_path_deviation.z <= 0.0) { + valid = false; + message += "+Invalid DoF Z max path deviation"; + } + if (params.max_path_deviation.a <= 0.0) { + valid = false; + message += "+Invalid DoF A max path deviation"; + } + if (params.max_path_deviation.b <= 0.0) { + valid = false; + message += "+Invalid DoF B max path deviation"; + } + if (params.max_path_deviation.c <= 0.0) { + valid = false; + message += "+Invalid DoF C max path deviation"; + } + + // Cartesian velocity + if (params.max_cartesian_velocity.x <= 0.0) { + valid = false; + message += "+Invalid DoF X max cartesian velocity"; + } + if (params.max_cartesian_velocity.y <= 0.0) { + valid = false; + message += "+Invalid DoF Y max cartesian velocity"; + } + if (params.max_cartesian_velocity.z <= 0.0) { + valid = false; + message += "+Invalid DoF Z max cartesian velocity"; + } + if (params.max_cartesian_velocity.a <= 0.0) { + valid = false; + message += "+Invalid DoF A max cartesian velocity"; + } + if (params.max_cartesian_velocity.b <= 0.0) { + valid = false; + message += "+Invalid DoF B max cartesian velocity"; + } + if (params.max_cartesian_velocity.c <= 0.0) { + valid = false; + message += "+Invalid DoF C max cartesian velocity"; + } + + // Cartesian force + if (params.max_control_force.x <= 0.0) { + valid = false; + message += "+Invalid DoF X max control force"; + } + if (params.max_control_force.y <= 0.0) { + valid = false; + message += "+Invalid DoF Y max control force"; + } + if (params.max_control_force.z <= 0.0) { + valid = false; + message += "+Invalid DoF Z max control force"; + } + if (params.max_control_force.a <= 0.0) { + valid = false; + message += "+Invalid DoF A max control force"; + } + if (params.max_control_force.b <= 0.0) { + valid = false; + message += "+Invalid DoF B max control force"; + } + if (params.max_control_force.c <= 0.0) { + valid = false; + message += "+Invalid DoF C max control force"; + } + + return std::make_pair(valid, message); +} - //// ROS Callbacks to get and set the control mode as service calls //////////////////////////////////////////// - - bool MinimalArmWrapperInterface::setControlModeCallback( - msgs::SetControlMode::Request& req, - msgs::SetControlMode::Response& res) - { - Maybe::Maybe local_active_control_mode_copy; - { - std::lock_guard lock(control_mode_status_mutex_); - local_active_control_mode_copy = active_control_mode_; - } - if (local_active_control_mode_copy.Valid()) - { - const auto merged_command = mergeControlModeParameters(local_active_control_mode_copy.GetImmutable(), req.new_control_mode); - const auto validity_check = validateControlMode(merged_command); - if (validity_check.first) - { - iiwa_ptr_->SendControlModeCommandMessage(merged_command); - - // Loop waiting for a matching control mode to be parsed - bool control_mode_matches = false; - - const std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now(); - - do - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - end_time = std::chrono::steady_clock::now(); - std::lock_guard lock(control_mode_status_mutex_); - control_mode_matches = controlModeParamsEqual(merged_command, active_control_mode_.GetImmutable()); - } - while (!control_mode_matches && std::chrono::duration(end_time - start_time).count() < set_control_mode_timeout_); - - // Check the results of the timeout - if (control_mode_matches) - { - res.success = true; - res.message = "Control mode set successfully"; - } - else - { - res.success = false; - res.message = "Control mode could not be set in Sunrise within the timeout window of " + std::to_string(set_control_mode_timeout_); - } - } - else - { - res.success = false; - res.message = validity_check.second; - } - } - else - { - res.success = false; - res.message = "No initial control mode available from the controller"; - } - return true; - } +std::pair MinimalArmWrapperInterface::validateControlMode( + const msgs::ControlModeParameters& params) { + bool valid = true; + std::string message; + + // Check the control mode itself + if (params.control_mode.mode != msgs::ControlMode::JOINT_POSITION && + params.control_mode.mode != msgs::ControlMode::JOINT_IMPEDANCE && + params.control_mode.mode != msgs::ControlMode::CARTESIAN_POSE && + params.control_mode.mode != msgs::ControlMode::CARTESIAN_IMPEDANCE) { + valid = false; + message += "+Invalid control mode"; + } + + // Check each part of the control mode + const auto valid_joint_impedance_params = validateJointImpedanceParams(params.joint_impedance_params); + const auto valid_cartesian_impedance_params = validateCartesianImpedanceParams(params.cartesian_impedance_params); + const auto valid_cartesian_control_mode_limits = + validateCartesianControlModeLimits(params.cartesian_control_mode_limits); + const auto valid_joint_path_execution_params = validateJointPathExecutionParams(params.joint_path_execution_params); + const auto valid_cartesian_path_execution_params = + validateCartesianPathExecutionParams(params.cartesian_path_execution_params); + + // Aggregate the results + valid &= valid_joint_impedance_params.first && valid_cartesian_impedance_params.first && + valid_cartesian_control_mode_limits.first && valid_joint_path_execution_params.first && + valid_cartesian_path_execution_params.first; + + message += valid_joint_impedance_params.second + valid_cartesian_impedance_params.second + + valid_cartesian_control_mode_limits.second + valid_joint_path_execution_params.second + + valid_cartesian_path_execution_params.second; + + return std::make_pair(valid, message); +} - bool MinimalArmWrapperInterface::getControlModeCallback( - msgs::GetControlMode::Request& req, - msgs::GetControlMode::Response& res) - { - UNUSED(req); +//// ROS Callbacks to get and set the control mode as service calls //////////////////////////////////////////// + +bool MinimalArmWrapperInterface::setControlModeCallback(msgs::SetControlMode::Request& req, + msgs::SetControlMode::Response& res) { + Maybe::Maybe local_active_control_mode_copy; + { + std::lock_guard lock(control_mode_status_mutex_); + local_active_control_mode_copy = active_control_mode_; + } + if (local_active_control_mode_copy.Valid()) { + const auto merged_command = + mergeControlModeParameters(local_active_control_mode_copy.GetImmutable(), req.new_control_mode); + const auto validity_check = validateControlMode(merged_command); + if (validity_check.first) { + iiwa_ptr_->SendControlModeCommandMessage(merged_command); + + // Loop waiting for a matching control mode to be parsed + bool control_mode_matches = false; + + const std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now(); + + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + end_time = std::chrono::steady_clock::now(); std::lock_guard lock(control_mode_status_mutex_); - res.has_active_control_mode = active_control_mode_.Valid(); - if (res.has_active_control_mode) - { - res.active_control_mode = active_control_mode_.GetImmutable(); - } - return true; - } + control_mode_matches = controlModeParamsEqual(merged_command, active_control_mode_.GetImmutable()); + } while (!control_mode_matches && + std::chrono::duration(end_time - start_time).count() < set_control_mode_timeout_); + + // Check the results of the timeout + if (control_mode_matches) { + res.success = true; + res.message = "Control mode set successfully"; + } else { + res.success = false; + res.message = "Control mode could not be set in Sunrise within the timeout window of " + + std::to_string(set_control_mode_timeout_); + } + } else { + res.success = false; + res.message = validity_check.second; + } + } else { + res.success = false; + res.message = "No initial control mode available from the controller"; + } + return true; +} - /* - * Callback function used by the LCM subsystem when a control_mode_status message is received. Caches the value - * in active_control_mode_ for use by setControlModeCallback(...) and getControlModeCallback(...), as well as publishes - * the message on the correct ROS topic - */ - void MinimalArmWrapperInterface::controlModeStatusLCMCallback( - const msgs::ControlModeParameters& control_mode_status) - { - { - std::lock_guard lock(control_mode_status_mutex_); - if (active_control_mode_.Valid() == false) - { - ROS_INFO_NAMED(ros::this_node::getName(), "Initializing active_control_mode for the first time"); - } - active_control_mode_ = control_mode_status; - } - control_mode_status_pub_.publish(control_mode_status); - } +bool MinimalArmWrapperInterface::getControlModeCallback(msgs::GetControlMode::Request& req, + msgs::GetControlMode::Response& res) { + UNUSED(req); + std::lock_guard lock(control_mode_status_mutex_); + res.has_active_control_mode = active_control_mode_.Valid(); + if (res.has_active_control_mode) { + res.active_control_mode = active_control_mode_.GetImmutable(); + } + return true; +} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Arm movement/control and feedback functionality - //////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* + * Callback function used by the LCM subsystem when a control_mode_status message is received. Caches the value + * in active_control_mode_ for use by setControlModeCallback(...) and getControlModeCallback(...), as well as publishes + * the message on the correct ROS topic + */ +void MinimalArmWrapperInterface::controlModeStatusLCMCallback(const msgs::ControlModeParameters& control_mode_status) { + { + std::lock_guard lock(control_mode_status_mutex_); + if (!active_control_mode_.Valid()) { + ROS_INFO_NAMED(ros::this_node::getName(), "Initializing active_control_mode for the first time"); + } + active_control_mode_ = control_mode_status; + } + control_mode_status_pub_.publish(control_mode_status); +} - //// Static helpers to check parameters for being in valid ranges for the motion command /////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Arm movement/control and feedback functionality +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// - std::pair MinimalArmWrapperInterface::validateJointPositions( - const msgs::JointValueQuantity& positions) - { - //TODO: The function is not implemented yet; it should check that the commanded joint positions are within the joint limits of the arm - UNUSED(positions); - return std::make_pair(true, std::string("")); - } +//// Static helpers to check parameters for being in valid ranges for the motion command /////////////////////// - std::pair MinimalArmWrapperInterface::validateCartesianPose( - const geometry_msgs::Pose& pose, - const std::string& frame) const - { - bool valid = true; - std::string message; - - // Check to make sure the frame is correct - if (frame != cartesian_control_frame_) - { - valid = false; - message += "+Commanded cartesian pose has the wrong frame, " + frame + " given, " + cartesian_control_frame_ + " expected"; - } - - // Check to make sure the quaternion is well formed - { - const double quat_squared_norm = (pose.orientation.w * pose.orientation.w) - + (pose.orientation.x * pose.orientation.x) - + (pose.orientation.y * pose.orientation.y) - + (pose.orientation.z * pose.orientation.z); - const double error = std::fabs(1.0 - quat_squared_norm); - if (error > 1e-6) - { - valid = false; - message += "+Commanded cartesian pose quaternion is not normalized, squared norm = " + std::to_string(quat_squared_norm); - } - } - return std::make_pair(valid, message); - } +std::pair MinimalArmWrapperInterface::validateJointPositions( + const msgs::JointValueQuantity& positions) { + // TODO: The function is not implemented yet; it should check that the commanded joint positions are within the joint + // limits of the arm + UNUSED(positions); + return std::make_pair(true, std::string("")); +} - std::pairMinimalArmWrapperInterface::validateMotionCommand( - const msgs::MotionCommand& command) const - { - Maybe::Maybe active_control_mode_cached; - { - std::lock_guard lock(control_mode_status_mutex_); - active_control_mode_cached = active_control_mode_; - } - if (active_control_mode_cached.Valid()) - { - const uint8_t active_control_mode = active_control_mode_cached.GetImmutable().control_mode.mode; - const uint8_t command_motion_mode = command.control_mode.mode; - - // Note that this assumes the two messages use the same enums for each item, this is asserted in the constructor - if (active_control_mode != command_motion_mode) - { - return std::make_pair(false, std::string("Active control mode does not match commanded control mode")); - } - - switch (command_motion_mode) - { - case msgs::ControlMode::JOINT_POSITION: - case msgs::ControlMode::JOINT_IMPEDANCE: - return validateJointPositions(command.joint_position); - - case msgs::ControlMode::CARTESIAN_POSE: - case msgs::ControlMode::CARTESIAN_IMPEDANCE: - return validateCartesianPose(command.cartesian_pose, command.header.frame_id); - - default: - return std::make_pair(false, std::string("Invalid commanded control mode. This should not be possible"));; - } - } - else - { - return std::make_pair(false, std::string("No active control mode, cannot command motion")); - } - } +std::pair MinimalArmWrapperInterface::validateCartesianPose(const geometry_msgs::Pose& pose, + const std::string& frame) const { + bool valid = true; + std::string message; + + // Check to make sure the frame is correct + if (frame != cartesian_control_frame_) { + valid = false; + message += + "+Commanded cartesian pose has the wrong frame, " + frame + " given, " + cartesian_control_frame_ + " expected"; + } + + // Check to make sure the quaternion is well formed + { + const double quat_squared_norm = + (pose.orientation.w * pose.orientation.w) + (pose.orientation.x * pose.orientation.x) + + (pose.orientation.y * pose.orientation.y) + (pose.orientation.z * pose.orientation.z); + const double error = std::fabs(1.0 - quat_squared_norm); + if (error > 1e-6) { + valid = false; + message += + "+Commanded cartesian pose quaternion is not normalized, squared norm = " + std::to_string(quat_squared_norm); + } + } + return std::make_pair(valid, message); +} - /* - * ROS callback to parse a arm motion command, and pass it along to the LCM subsystem - */ - void MinimalArmWrapperInterface::motionCommandROSCallback(const msgs::MotionCommand& command) - { - const auto validity_check_results = validateMotionCommand(command); - if (validity_check_results.first) - { - iiwa_ptr_->SendMotionCommandMessage(command); - } - else - { - ROS_ERROR_STREAM_NAMED(ros::this_node::getName(), "Arm motion command failed validity checks: " << validity_check_results.second); - } - } +std::pair MinimalArmWrapperInterface::validateMotionCommand( + const msgs::MotionCommand& command) const { + Maybe::Maybe active_control_mode_cached; + { + std::lock_guard lock(control_mode_status_mutex_); + active_control_mode_cached = active_control_mode_; + } + if (active_control_mode_cached.Valid()) { + const uint8_t active_control_mode = active_control_mode_cached.GetImmutable().control_mode.mode; + const uint8_t command_motion_mode = command.control_mode.mode; + + // Note that this assumes the two messages use the same enums for each item, this is asserted in the constructor + if (active_control_mode != command_motion_mode) { + return std::make_pair(false, std::string("Active control mode does not match commanded control mode")); + } + + switch (command_motion_mode) { + case msgs::ControlMode::JOINT_POSITION: + case msgs::ControlMode::JOINT_IMPEDANCE: + return validateJointPositions(command.joint_position); + + case msgs::ControlMode::CARTESIAN_POSE: + case msgs::ControlMode::CARTESIAN_IMPEDANCE: + return validateCartesianPose(command.cartesian_pose, command.header.frame_id); + + default: + return std::make_pair(false, std::string("Invalid commanded control mode. This should not be possible")); + } + } else { + return std::make_pair(false, std::string("No active control mode, cannot command motion")); + } +} - /* - * LCM callback used by the LCM subsystem when a LCM status message is recieved. Republishes the motion status - * on the correct ROS topic - */ - void MinimalArmWrapperInterface::motionStatusLCMCallback(const msgs::MotionStatus& motion_status) - { - motion_status_pub_.publish(motion_status); - } +/* + * ROS callback to parse a arm motion command, and pass it along to the LCM subsystem + */ +void MinimalArmWrapperInterface::motionCommandROSCallback(const msgs::MotionCommand& command) { + const auto validity_check_results = validateMotionCommand(command); + if (validity_check_results.first) { + iiwa_ptr_->SendMotionCommandMessage(command); + } else { + ROS_ERROR_STREAM_NAMED(ros::this_node::getName(), + "Arm motion command failed validity checks: " << validity_check_results.second); + } +} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Gripper movement/control and feedback functionality - //////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - //// Static helpers to check parameters for being in valid ranges for the gripper command ////////////////////// - - std::pair MinimalArmWrapperInterface::validateFingerCommand( - const msgs::Robotiq3FingerActuatorCommand& command) - { - bool valid = true; - std::string message; - if (command.position > 1.0 || command.position < 0.0) - { - valid = false; - message += "+Invalid finger position"; - } - - if (command.force > 1.0 || command.force < 0.0) - { - valid = false; - message += "+Invalid finger force"; - } - - if (command.speed > 1.0 || command.speed < 0.0) - { - valid = false; - message += "+Invalid finger speed"; - } - - return std::make_pair(valid, message); - } +/* + * LCM callback used by the LCM subsystem when a LCM status message is recieved. Republishes the motion status + * on the correct ROS topic + */ +void MinimalArmWrapperInterface::motionStatusLCMCallback(const msgs::MotionStatus& motion_status) { + motion_status_pub_.publish(motion_status); +} - std::pair MinimalArmWrapperInterface::validateGripperCommand( - const msgs::Robotiq3FingerCommand& command) - { - const auto ac = validateFingerCommand(command.finger_a_command); - const auto bc = validateFingerCommand(command.finger_b_command); - const auto cc = validateFingerCommand(command.finger_c_command); - const auto sc = validateFingerCommand(command.scissor_command); - - const bool valid = ac.first && bc.first && cc.first && sc.first; - const std::string message = ac.second + bc.second + cc.second + sc.second; - return std::make_pair(valid, message); - } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Gripper movement/control and feedback functionality +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /* - * ROS callback to parse a gripper motion command, and pass it along to the LCM subsystem - */ - void MinimalArmWrapperInterface::gripperCommandROSCallback( - const msgs::Robotiq3FingerCommand& command) - { - const auto validity_check_results = validateGripperCommand(command); - if (validity_check_results.first) - { - robotiq_ptr_->sendCommandMessage(command); - } - else - { - ROS_WARN_STREAM_NAMED(ros::this_node::getName(), "Gripper command failed validity checks: " << validity_check_results.second); - } - } +//// Static helpers to check parameters for being in valid ranges for the gripper command ////////////////////// - /* - * LCM callback used by the LCM subsystem when a LCM status message is recieved. Republishes the motion status - * on the correct ROS topic - */ - void MinimalArmWrapperInterface::gripperStatusLCMCallback( - const msgs::Robotiq3FingerStatus& gripper_status) - { - gripper_status_pub_.publish(gripper_status); - } +std::pair MinimalArmWrapperInterface::validateFingerCommand( + const msgs::Robotiq3FingerActuatorCommand& command) { + bool valid = true; + std::string message; + if (command.position > 1.0 || command.position < 0.0) { + valid = false; + message += "+Invalid finger position"; + } - //////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Internal helpers - //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + if (command.force > 1.0 || command.force < 0.0) { + valid = false; + message += "+Invalid finger force"; + } - inline bool jointPathExecutionParamsIsDefault(const msgs::JointPathExecutionParameters& params) - { - return (params.joint_relative_velocity == 0 && - params.joint_relative_acceleration == 0 && - params.override_joint_acceleration == 0); - } + if (command.speed > 1.0 || command.speed < 0.0) { + valid = false; + message += "+Invalid finger speed"; + } - inline bool cartesianPathExecutionParamsIsDefault(const msgs::CartesianPathExecutionParameters& params) - { - return (params.max_velocity.x == 0 && params.max_velocity.y == 0 && - params.max_velocity.z == 0 && params.max_velocity.a == 0 && - params.max_velocity.b == 0 && params.max_velocity.c == 0 && - params.max_acceleration.x == 0 && params.max_acceleration.y == 0 && - params.max_acceleration.z == 0 && params.max_acceleration.a == 0 && - params.max_acceleration.b == 0 && params.max_acceleration.c == 0 && - params.max_nullspace_velocity == 0 && params.max_nullspace_acceleration == 0); - } + return std::make_pair(valid, message); +} - inline bool jointImpedanceParamsIsDefault(const msgs::JointImpedanceParameters& params) - { - return (params.joint_stiffness.joint_1 == 0 && params.joint_stiffness.joint_2 == 0 && - params.joint_stiffness.joint_3 == 0 && params.joint_stiffness.joint_4 == 0 && - params.joint_stiffness.joint_5 == 0 && params.joint_stiffness.joint_6 == 0 && - params.joint_stiffness.joint_7 == 0 && - params.joint_damping.joint_1 == 0 && params.joint_damping.joint_2 == 0 && - params.joint_damping.joint_3 == 0 && params.joint_damping.joint_4 == 0 && - params.joint_damping.joint_5 == 0 && params.joint_damping.joint_6 == 0 && - params.joint_damping.joint_7 == 0); - } +std::pair MinimalArmWrapperInterface::validateGripperCommand( + const msgs::Robotiq3FingerCommand& command) { + const auto ac = validateFingerCommand(command.finger_a_command); + const auto bc = validateFingerCommand(command.finger_b_command); + const auto cc = validateFingerCommand(command.finger_c_command); + const auto sc = validateFingerCommand(command.scissor_command); - inline bool cartesianImpedanceParamsIsDefault(const msgs::CartesianImpedanceParameters& params) - { - return (params.cartesian_stiffness.x == 0 && params.cartesian_stiffness.y == 0 && - params.cartesian_stiffness.z == 0 && params.cartesian_stiffness.a == 0 && - params.cartesian_stiffness.b == 0 && params.cartesian_stiffness.c == 0 && - params.cartesian_damping.x == 0 && params.cartesian_damping.y == 0 && - params.cartesian_damping.z == 0 && params.cartesian_damping.a == 0 && - params.cartesian_damping.b == 0 && params.cartesian_damping.c == 0 && - params.nullspace_stiffness == 0 && params.nullspace_damping == 0); - } + const bool valid = ac.first && bc.first && cc.first && sc.first; + const std::string message = ac.second + bc.second + cc.second + sc.second; + return std::make_pair(valid, message); +} - inline bool cartesianControlModeLimitsIsDefault(const msgs::CartesianControlModeLimits& params) - { - return (params.max_path_deviation.x == 0 && params.max_path_deviation.y == 0 && - params.max_path_deviation.z == 0 && params.max_path_deviation.a == 0 && - params.max_path_deviation.b == 0 && params.max_path_deviation.c == 0 && - params.max_cartesian_velocity.x == 0 && params.max_cartesian_velocity.y == 0 && - params.max_cartesian_velocity.z == 0 && params.max_cartesian_velocity.a == 0 && - params.max_cartesian_velocity.b == 0 && params.max_cartesian_velocity.c == 0 && - params.max_control_force.x == 0 && params.max_control_force.y == 0 && - params.max_control_force.z == 0 && params.max_control_force.a == 0 && - params.max_control_force.b == 0 && params.max_control_force.c == 0 && - params.stop_on_max_control_force == false); - } +/* + * ROS callback to parse a gripper motion command, and pass it along to the LCM subsystem + */ +void MinimalArmWrapperInterface::gripperCommandROSCallback(const msgs::Robotiq3FingerCommand& command) { + const auto validity_check_results = validateGripperCommand(command); + if (validity_check_results.first) { + robotiq_ptr_->sendCommandMessage(command); + } else { + ROS_WARN_STREAM_NAMED(ros::this_node::getName(), + "Gripper command failed validity checks: " << validity_check_results.second); + } +} - inline msgs::ControlModeParameters mergeControlModeParameters( - const msgs::ControlModeParameters& active_control_mode, - const msgs::ControlModeParameters& new_control_mode) - { - /*************************************************************************************************************** - This function is a helper function for the callback function of setting a new control mode(setControlModeCallBack). - It copies the parameters of the old control mode to the new one, and updates relevant parameters with theparameters - of the new control mode. - - Parameters updated in each control mode: - JOINT_POSITION: joint_path_execution_params - CARTESIAN_POSE: cartesian_path_execution_params - JOINT_IMPEDANCE: joint_impedance_params, joint_path_execution_params - CARTESIAN_IMPEDANCE: cartesian_impedance_params, cartesian_control_mode_limits, cartesian_path_execution_params - ***************************************************************************************************************/ - - msgs::ControlModeParameters merged_control_mode; - // Copy the old over - merged_control_mode.joint_path_execution_params = active_control_mode.joint_path_execution_params; - merged_control_mode.joint_impedance_params = active_control_mode.joint_impedance_params; - merged_control_mode.cartesian_impedance_params = active_control_mode.cartesian_impedance_params; - merged_control_mode.cartesian_control_mode_limits = active_control_mode.cartesian_control_mode_limits; - merged_control_mode.cartesian_path_execution_params = active_control_mode.cartesian_path_execution_params; - // Copy manadatory members - merged_control_mode.control_mode = new_control_mode.control_mode; - // Copy mode-dependant members - switch (new_control_mode.control_mode.mode) - { - case msgs::ControlMode::JOINT_IMPEDANCE: - merged_control_mode.joint_path_execution_params = new_control_mode.joint_path_execution_params; - merged_control_mode.joint_impedance_params = new_control_mode.joint_impedance_params; - - if (!cartesianImpedanceParamsIsDefault(new_control_mode.cartesian_impedance_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The cartesian impedance parameters are specified but ignored in JOINT_IMPEDANCE mode."); - } - if (!cartesianControlModeLimitsIsDefault(new_control_mode.cartesian_control_mode_limits)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The cartesian control mode limits are specified but ignored in JOINT_IMPEDANCE mode."); - } - if (!cartesianPathExecutionParamsIsDefault(new_control_mode.cartesian_path_execution_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The cartesian path execution parameters are specified but ignored in JOINT_IMPEDANCE mode."); - } - - break; - - case msgs::ControlMode::CARTESIAN_IMPEDANCE: - merged_control_mode.cartesian_impedance_params = new_control_mode.cartesian_impedance_params; - merged_control_mode.cartesian_control_mode_limits = new_control_mode.cartesian_control_mode_limits; - merged_control_mode.cartesian_path_execution_params = new_control_mode.cartesian_path_execution_params; - - if (!jointPathExecutionParamsIsDefault(new_control_mode.joint_path_execution_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The joint path execution parameters are specified but ignored in CASRTESIAN_IMPEDANCE mode."); - } - if (!jointImpedanceParamsIsDefault(new_control_mode.joint_impedance_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The joint impedance parameters are specified but ignored in CASRTESIAN_IMPEDANCE mode."); - } - - break; - - case msgs::ControlMode::JOINT_POSITION: - // From the new - merged_control_mode.joint_path_execution_params = new_control_mode.joint_path_execution_params; - - if (!jointImpedanceParamsIsDefault(new_control_mode.joint_impedance_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The joint impedance parameters are specified but ignored in JOINT_POSITION mode."); - } - if (!cartesianImpedanceParamsIsDefault(new_control_mode.cartesian_impedance_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The cartesian impedance parameters are specified but ignored in JOINT_POSITION mode."); - } - if (!cartesianControlModeLimitsIsDefault(new_control_mode.cartesian_control_mode_limits)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The cartesian control mode limits are specified but ignored in JOINT_POSITION mode."); - } - if (!cartesianPathExecutionParamsIsDefault(new_control_mode.cartesian_path_execution_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The cartesian path execution parameters are specified but ignored in JOINT_POSITION mode."); - } - - break; - - case msgs::ControlMode::CARTESIAN_POSE: - // From the new - merged_control_mode.cartesian_path_execution_params = new_control_mode.cartesian_path_execution_params; - - if (!jointPathExecutionParamsIsDefault(new_control_mode.joint_path_execution_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The joint path execution parameters are specified but ignored in CARTESIAN_POSE mode."); - } - if (!jointImpedanceParamsIsDefault(new_control_mode.joint_impedance_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The joint impedance parameters are specified but ignored in CARTESIAN_POSE mode."); - } - if (!cartesianImpedanceParamsIsDefault(new_control_mode.cartesian_impedance_params)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The cartesian impedance parameters are specified but ignored in CARTESIAN_POSE mode."); - } - if (!cartesianControlModeLimitsIsDefault(new_control_mode.cartesian_control_mode_limits)) - { - ROS_WARN_NAMED(ros::this_node::getName(), "The cartesian control mode limits are specified but ignored in CARTESIAN_POSE mode."); - } - - break; - - default: - ROS_INFO_STREAM_NAMED(ros::this_node::getName(), "Invalid control mode: " << new_control_mode.control_mode << "."); - assert(false); - - break; - - } - - return merged_control_mode; - } +/* + * LCM callback used by the LCM subsystem when a LCM status message is recieved. Republishes the motion status + * on the correct ROS topic + */ +void MinimalArmWrapperInterface::gripperStatusLCMCallback(const msgs::Robotiq3FingerStatus& gripper_status) { + gripper_status_pub_.publish(gripper_status); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Internal helpers +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +inline bool jointPathExecutionParamsIsDefault(const msgs::JointPathExecutionParameters& params) { + return (params.joint_relative_velocity == 0 && params.joint_relative_acceleration == 0 && + params.override_joint_acceleration == 0); +} + +inline bool cartesianPathExecutionParamsIsDefault(const msgs::CartesianPathExecutionParameters& params) { + return (params.max_velocity.x == 0 && params.max_velocity.y == 0 && params.max_velocity.z == 0 && + params.max_velocity.a == 0 && params.max_velocity.b == 0 && params.max_velocity.c == 0 && + params.max_acceleration.x == 0 && params.max_acceleration.y == 0 && params.max_acceleration.z == 0 && + params.max_acceleration.a == 0 && params.max_acceleration.b == 0 && params.max_acceleration.c == 0 && + params.max_nullspace_velocity == 0 && params.max_nullspace_acceleration == 0); +} + +inline bool jointImpedanceParamsIsDefault(const msgs::JointImpedanceParameters& params) { + return (params.joint_stiffness.joint_1 == 0 && params.joint_stiffness.joint_2 == 0 && + params.joint_stiffness.joint_3 == 0 && params.joint_stiffness.joint_4 == 0 && + params.joint_stiffness.joint_5 == 0 && params.joint_stiffness.joint_6 == 0 && + params.joint_stiffness.joint_7 == 0 && params.joint_damping.joint_1 == 0 && + params.joint_damping.joint_2 == 0 && params.joint_damping.joint_3 == 0 && params.joint_damping.joint_4 == 0 && + params.joint_damping.joint_5 == 0 && params.joint_damping.joint_6 == 0 && params.joint_damping.joint_7 == 0); +} + +inline bool cartesianImpedanceParamsIsDefault(const msgs::CartesianImpedanceParameters& params) { + return (params.cartesian_stiffness.x == 0 && params.cartesian_stiffness.y == 0 && params.cartesian_stiffness.z == 0 && + params.cartesian_stiffness.a == 0 && params.cartesian_stiffness.b == 0 && params.cartesian_stiffness.c == 0 && + params.cartesian_damping.x == 0 && params.cartesian_damping.y == 0 && params.cartesian_damping.z == 0 && + params.cartesian_damping.a == 0 && params.cartesian_damping.b == 0 && params.cartesian_damping.c == 0 && + params.nullspace_stiffness == 0 && params.nullspace_damping == 0); +} + +inline bool cartesianControlModeLimitsIsDefault(const msgs::CartesianControlModeLimits& params) { + return (params.max_path_deviation.x == 0 && params.max_path_deviation.y == 0 && params.max_path_deviation.z == 0 && + params.max_path_deviation.a == 0 && params.max_path_deviation.b == 0 && params.max_path_deviation.c == 0 && + params.max_cartesian_velocity.x == 0 && params.max_cartesian_velocity.y == 0 && + params.max_cartesian_velocity.z == 0 && params.max_cartesian_velocity.a == 0 && + params.max_cartesian_velocity.b == 0 && params.max_cartesian_velocity.c == 0 && + params.max_control_force.x == 0 && params.max_control_force.y == 0 && params.max_control_force.z == 0 && + params.max_control_force.a == 0 && params.max_control_force.b == 0 && params.max_control_force.c == 0 && + params.stop_on_max_control_force == false); +} + +inline msgs::ControlModeParameters mergeControlModeParameters(const msgs::ControlModeParameters& active_control_mode, + const msgs::ControlModeParameters& new_control_mode) { + /*************************************************************************************************************** + This function is a helper function for the callback function of setting a new control mode(setControlModeCallBack). + It copies the parameters of the old control mode to the new one, and updates relevant parameters with theparameters + of the new control mode. + + Parameters updated in each control mode: + JOINT_POSITION: joint_path_execution_params + CARTESIAN_POSE: cartesian_path_execution_params + JOINT_IMPEDANCE: joint_impedance_params, joint_path_execution_params + CARTESIAN_IMPEDANCE: cartesian_impedance_params, cartesian_control_mode_limits, cartesian_path_execution_params + ***************************************************************************************************************/ + + msgs::ControlModeParameters merged_control_mode; + // Copy the old over + merged_control_mode.joint_path_execution_params = active_control_mode.joint_path_execution_params; + merged_control_mode.joint_impedance_params = active_control_mode.joint_impedance_params; + merged_control_mode.cartesian_impedance_params = active_control_mode.cartesian_impedance_params; + merged_control_mode.cartesian_control_mode_limits = active_control_mode.cartesian_control_mode_limits; + merged_control_mode.cartesian_path_execution_params = active_control_mode.cartesian_path_execution_params; + // Copy manadatory members + merged_control_mode.control_mode = new_control_mode.control_mode; + // Copy mode-dependant members + switch (new_control_mode.control_mode.mode) { + case msgs::ControlMode::JOINT_IMPEDANCE: + merged_control_mode.joint_path_execution_params = new_control_mode.joint_path_execution_params; + merged_control_mode.joint_impedance_params = new_control_mode.joint_impedance_params; + + if (!cartesianImpedanceParamsIsDefault(new_control_mode.cartesian_impedance_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The cartesian impedance parameters are specified but ignored in JOINT_IMPEDANCE mode."); + } + if (!cartesianControlModeLimitsIsDefault(new_control_mode.cartesian_control_mode_limits)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The cartesian control mode limits are specified but ignored in JOINT_IMPEDANCE mode."); + } + if (!cartesianPathExecutionParamsIsDefault(new_control_mode.cartesian_path_execution_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The cartesian path execution parameters are specified but ignored in JOINT_IMPEDANCE mode."); + } + + break; + + case msgs::ControlMode::CARTESIAN_IMPEDANCE: + merged_control_mode.cartesian_impedance_params = new_control_mode.cartesian_impedance_params; + merged_control_mode.cartesian_control_mode_limits = new_control_mode.cartesian_control_mode_limits; + merged_control_mode.cartesian_path_execution_params = new_control_mode.cartesian_path_execution_params; + + if (!jointPathExecutionParamsIsDefault(new_control_mode.joint_path_execution_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The joint path execution parameters are specified but ignored in CASRTESIAN_IMPEDANCE mode."); + } + if (!jointImpedanceParamsIsDefault(new_control_mode.joint_impedance_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The joint impedance parameters are specified but ignored in CASRTESIAN_IMPEDANCE mode."); + } + + break; + + case msgs::ControlMode::JOINT_POSITION: + // From the new + merged_control_mode.joint_path_execution_params = new_control_mode.joint_path_execution_params; + + if (!jointImpedanceParamsIsDefault(new_control_mode.joint_impedance_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The joint impedance parameters are specified but ignored in JOINT_POSITION mode."); + } + if (!cartesianImpedanceParamsIsDefault(new_control_mode.cartesian_impedance_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The cartesian impedance parameters are specified but ignored in JOINT_POSITION mode."); + } + if (!cartesianControlModeLimitsIsDefault(new_control_mode.cartesian_control_mode_limits)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The cartesian control mode limits are specified but ignored in JOINT_POSITION mode."); + } + if (!cartesianPathExecutionParamsIsDefault(new_control_mode.cartesian_path_execution_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The cartesian path execution parameters are specified but ignored in JOINT_POSITION mode."); + } + + break; + + case msgs::ControlMode::CARTESIAN_POSE: + // From the new + merged_control_mode.cartesian_path_execution_params = new_control_mode.cartesian_path_execution_params; + + if (!jointPathExecutionParamsIsDefault(new_control_mode.joint_path_execution_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The joint path execution parameters are specified but ignored in CARTESIAN_POSE mode."); + } + if (!jointImpedanceParamsIsDefault(new_control_mode.joint_impedance_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The joint impedance parameters are specified but ignored in CARTESIAN_POSE mode."); + } + if (!cartesianImpedanceParamsIsDefault(new_control_mode.cartesian_impedance_params)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The cartesian impedance parameters are specified but ignored in CARTESIAN_POSE mode."); + } + if (!cartesianControlModeLimitsIsDefault(new_control_mode.cartesian_control_mode_limits)) { + ROS_WARN_NAMED(ros::this_node::getName(), + "The cartesian control mode limits are specified but ignored in CARTESIAN_POSE mode."); + } + + break; + + default: + ROS_INFO_STREAM_NAMED(ros::this_node::getName(), + "Invalid control mode: " << new_control_mode.control_mode << "."); + assert(false); + } + + return merged_control_mode; } +} // namespace victor_hardware_interface diff --git a/victor_hardware_interface/src/victor_hardware_interface/robotiq_3finger_hardware_interface.cpp b/victor_hardware_interface/src/victor_hardware_interface/robotiq_3finger_hardware_interface.cpp index 82611871..4963ef17 100644 --- a/victor_hardware_interface/src/victor_hardware_interface/robotiq_3finger_hardware_interface.cpp +++ b/victor_hardware_interface/src/victor_hardware_interface/robotiq_3finger_hardware_interface.cpp @@ -1,125 +1,109 @@ #include +#include -namespace victor_hardware_interface -{ - namespace msgs = victor_hardware_interface_msgs; +namespace victor_hardware_interface { +namespace msgs = victor_hardware_interface_msgs; - ///////////////////////////////////////////////////////////////////////////////// - // Robotiq3FingerHardwardInterface class implementation - ///////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////// +// Robotiq3FingerHardwardInterface class implementation +///////////////////////////////////////////////////////////////////////////////// - Robotiq3FingerHardwareInterface::Robotiq3FingerHardwareInterface( - const std::shared_ptr& send_lcm_ptr, - const std::shared_ptr& recv_lcm_ptr, - const std::string& command_channel_name, - const std::string& status_channel_name, - const std::function& status_callback_fn) - : send_lcm_ptr_(send_lcm_ptr), recv_lcm_ptr_(recv_lcm_ptr) - , command_channel_name_(command_channel_name) - , status_channel_name_(status_channel_name) - , status_callback_fn_(status_callback_fn) - { - // Check lcm is valid to communicating - if (send_lcm_ptr_->good() != true) - { - throw std::invalid_argument("Send LCM interface is not good"); - } - if (recv_lcm_ptr_->good() != true) - { - throw std::invalid_argument("Receive LCM interface is not good"); - } - recv_lcm_ptr_->subscribe(status_channel_name_, - &Robotiq3FingerHardwareInterface::internalStatusLCMCallback, - this); - } - - bool Robotiq3FingerHardwareInterface::sendCommandMessage(const msgs::Robotiq3FingerCommand& command) - { - const robotiq_3finger_command lcm_command = commandRosToLcm(command); - const int ret = send_lcm_ptr_->publish(command_channel_name_, &lcm_command); - if (ret == 0) - { - return true; - } - else - { - return false; - } - } +Robotiq3FingerHardwareInterface::Robotiq3FingerHardwareInterface( + std::shared_ptr send_lcm_ptr, std::shared_ptr recv_lcm_ptr, + std::string command_channel_name, std::string status_channel_name, + std::function status_callback_fn) + : send_lcm_ptr_(std::move(send_lcm_ptr)), + recv_lcm_ptr_(std::move(recv_lcm_ptr)), + command_channel_name_(std::move(command_channel_name)), + status_channel_name_(std::move(status_channel_name)), + status_callback_fn_(std::move(status_callback_fn)) { + // Check lcm is valid to communicating + if (!send_lcm_ptr_->good()) { + throw std::invalid_argument("Send LCM interface is not good"); + } + if (!recv_lcm_ptr_->good()) { + throw std::invalid_argument("Receive LCM interface is not good"); + } + recv_lcm_ptr_->subscribe(status_channel_name_, &Robotiq3FingerHardwareInterface::internalStatusLCMCallback, this); +} - void Robotiq3FingerHardwareInterface::internalStatusLCMCallback(const lcm::ReceiveBuffer* buffer, - const std::string& channel, - const robotiq_3finger_status* status_msg) - { - UNUSED(buffer); - UNUSED(channel); - const msgs::Robotiq3FingerStatus ros_status = statusLcmToRos(*status_msg); - status_callback_fn_(ros_status); - } +bool Robotiq3FingerHardwareInterface::sendCommandMessage(const msgs::Robotiq3FingerCommand& command) { + const robotiq_3finger_command lcm_command = commandRosToLcm(command); + const int ret = send_lcm_ptr_->publish(command_channel_name_, &lcm_command); + if (ret == 0) { + return true; + } else { + return false; + } +} - ///////////////////////////////////////////////////////////////////////////////// - // Ros and LCM convert helper functions - ///////////////////////////////////////////////////////////////////////////////// +void Robotiq3FingerHardwareInterface::internalStatusLCMCallback(const lcm::ReceiveBuffer* buffer, + const std::string& channel, + const robotiq_3finger_status* status_msg) { + UNUSED(buffer); + UNUSED(channel); + const msgs::Robotiq3FingerStatus ros_status = statusLcmToRos(*status_msg); + status_callback_fn_(ros_status); +} - robotiq_3finger_actuator_command fingerCommandRosToLcm(const msgs::Robotiq3FingerActuatorCommand& finger_command) - { - robotiq_3finger_actuator_command lcm_command; +///////////////////////////////////////////////////////////////////////////////// +// Ros and LCM convert helper functions +///////////////////////////////////////////////////////////////////////////////// - // "Magic" number 0.0 and 1 are the bounds representing percentage value - lcm_command.position = arc_helpers::ClampValueAndWarn(finger_command.position, 0.0, 1.0); - lcm_command.speed = arc_helpers::ClampValueAndWarn(finger_command.speed, 0.0, 1.0); - lcm_command.force = arc_helpers::ClampValueAndWarn(finger_command.force, 0.0, 1.0); - lcm_command.timestamp = finger_command.header.stamp.toSec(); - return lcm_command; - } +robotiq_3finger_actuator_command fingerCommandRosToLcm(const msgs::Robotiq3FingerActuatorCommand& finger_command) { + robotiq_3finger_actuator_command lcm_command{}; - msgs::Robotiq3FingerActuatorStatus fingerStatusLcmToRos(const robotiq_3finger_actuator_status& finger_status) - { - msgs::Robotiq3FingerActuatorStatus ros_status; - ros_status.position = finger_status.position; - ros_status.position_request = finger_status.position_request; - ros_status.current = finger_status.current; - ros_status.header.stamp = ros::Time(finger_status.timestamp); - return ros_status; - } + // "Magic" number 0.0 and 1 are the bounds representing percentage value + lcm_command.position = arc_helpers::ClampValueAndWarn(finger_command.position, 0.0, 1.0); + lcm_command.speed = arc_helpers::ClampValueAndWarn(finger_command.speed, 0.0, 1.0); + lcm_command.force = arc_helpers::ClampValueAndWarn(finger_command.force, 0.0, 1.0); + lcm_command.timestamp = finger_command.header.stamp.toSec(); + return lcm_command; +} - msgs::Robotiq3FingerObjectStatus objectStatusLcmToRos(const robotiq_3finger_object_status& object_status) - { - msgs::Robotiq3FingerObjectStatus ros_status; - ros_status.status = (uint8_t)object_status.status; - ros_status.header.stamp = ros::Time(object_status.timestamp); - return ros_status; - } +msgs::Robotiq3FingerActuatorStatus fingerStatusLcmToRos(const robotiq_3finger_actuator_status& finger_status) { + msgs::Robotiq3FingerActuatorStatus ros_status; + ros_status.position = finger_status.position; + ros_status.position_request = finger_status.position_request; + ros_status.current = finger_status.current; + ros_status.header.stamp = ros::Time(finger_status.timestamp); + return ros_status; +} - msgs::Robotiq3FingerStatus statusLcmToRos(const robotiq_3finger_status& status) - { - msgs::Robotiq3FingerStatus ros_status; - ros_status.finger_a_status = fingerStatusLcmToRos(status.finger_a_status); - ros_status.finger_b_status = fingerStatusLcmToRos(status.finger_b_status); - ros_status.finger_c_status = fingerStatusLcmToRos(status.finger_c_status); - ros_status.scissor_status = fingerStatusLcmToRos(status.scissor_status); - ros_status.finger_a_object_status = objectStatusLcmToRos(status.finger_a_object_status); - ros_status.finger_b_object_status = objectStatusLcmToRos(status.finger_b_object_status); - ros_status.finger_c_object_status = objectStatusLcmToRos(status.finger_c_object_status); - ros_status.scissor_object_status = objectStatusLcmToRos(status.scissor_object_status); - ros_status.gripper_action_status = (uint8_t)status.gripper_action_status; - ros_status.gripper_system_status = (uint8_t)status.gripper_system_status; - ros_status.gripper_motion_status = (uint8_t)status.gripper_motion_status; - ros_status.gripper_fault_status = (uint8_t)status.gripper_fault_status; - ros_status.initialization_status = (uint8_t)status.initialization_status; - ros_status.header.stamp = ros::Time(status.timestamp); - return ros_status; - } +msgs::Robotiq3FingerObjectStatus objectStatusLcmToRos(const robotiq_3finger_object_status& object_status) { + msgs::Robotiq3FingerObjectStatus ros_status; + ros_status.status = (uint8_t)object_status.status; + ros_status.header.stamp = ros::Time(object_status.timestamp); + return ros_status; +} - robotiq_3finger_command commandRosToLcm(const msgs::Robotiq3FingerCommand& command) - { - robotiq_3finger_command lcm_command; - lcm_command.finger_a_command = fingerCommandRosToLcm(command.finger_a_command); - lcm_command.finger_b_command = fingerCommandRosToLcm(command.finger_b_command); - lcm_command.finger_c_command = fingerCommandRosToLcm(command.finger_c_command); - lcm_command.scissor_command = fingerCommandRosToLcm(command.scissor_command); - lcm_command.timestamp = command.header.stamp.toSec(); - return lcm_command; - } +msgs::Robotiq3FingerStatus statusLcmToRos(const robotiq_3finger_status& status) { + msgs::Robotiq3FingerStatus ros_status; + ros_status.finger_a_status = fingerStatusLcmToRos(status.finger_a_status); + ros_status.finger_b_status = fingerStatusLcmToRos(status.finger_b_status); + ros_status.finger_c_status = fingerStatusLcmToRos(status.finger_c_status); + ros_status.scissor_status = fingerStatusLcmToRos(status.scissor_status); + ros_status.finger_a_object_status = objectStatusLcmToRos(status.finger_a_object_status); + ros_status.finger_b_object_status = objectStatusLcmToRos(status.finger_b_object_status); + ros_status.finger_c_object_status = objectStatusLcmToRos(status.finger_c_object_status); + ros_status.scissor_object_status = objectStatusLcmToRos(status.scissor_object_status); + ros_status.gripper_action_status = (uint8_t)status.gripper_action_status; + ros_status.gripper_system_status = (uint8_t)status.gripper_system_status; + ros_status.gripper_motion_status = (uint8_t)status.gripper_motion_status; + ros_status.gripper_fault_status = (uint8_t)status.gripper_fault_status; + ros_status.initialization_status = (uint8_t)status.initialization_status; + ros_status.header.stamp = ros::Time(status.timestamp); + return ros_status; +} +robotiq_3finger_command commandRosToLcm(const msgs::Robotiq3FingerCommand& command) { + robotiq_3finger_command lcm_command{}; + lcm_command.finger_a_command = fingerCommandRosToLcm(command.finger_a_command); + lcm_command.finger_b_command = fingerCommandRosToLcm(command.finger_b_command); + lcm_command.finger_c_command = fingerCommandRosToLcm(command.finger_c_command); + lcm_command.scissor_command = fingerCommandRosToLcm(command.scissor_command); + lcm_command.timestamp = command.header.stamp.toSec(); + return lcm_command; } + +} // namespace victor_hardware_interface diff --git a/victor_hardware_interface/src/victor_hardware_interface/victor_utils.py b/victor_hardware_interface/src/victor_hardware_interface/victor_utils.py index c03c10e2..11cc3b10 100644 --- a/victor_hardware_interface/src/victor_hardware_interface/victor_utils.py +++ b/victor_hardware_interface/src/victor_hardware_interface/victor_utils.py @@ -2,6 +2,8 @@ from enum import Enum +from typing import List, Sequence + import rospy from victor_hardware_interface_msgs.msg import * from victor_hardware_interface_msgs.srv import * @@ -54,6 +56,7 @@ def get_joint_position_params(vel, accel): new_control_mode.control_mode.mode = ControlMode.JOINT_POSITION new_control_mode.joint_path_execution_params.joint_relative_velocity = vel new_control_mode.joint_path_execution_params.joint_relative_acceleration = accel + new_control_mode.joint_path_execution_params.override_joint_acceleration = 0.0 return new_control_mode @@ -211,6 +214,23 @@ def list_to_jvq(quantity_list): return jvq +def gripper_status_to_list(gripper_status: Robotiq3FingerStatus) -> List[float]: + return [gripper_status.finger_a_status.position, + gripper_status.finger_b_status.position, + gripper_status.finger_c_status.position, + gripper_status.scissor_status.position] + + +def list_to_gripper_status(gripper_status_list: Sequence[float]) -> Robotiq3FingerStatus: + assert len(gripper_status_list) == 4, "gripper status list must be length 4" + gripper_status = Robotiq3FingerStatus() + gripper_status.finger_a_status.position = gripper_status_list[0] + gripper_status.finger_b_status.position = gripper_status_list[1] + gripper_status.finger_c_status.position = gripper_status_list[2] + gripper_status.scissor_status.position = gripper_status_list[3] + return gripper_status + + def default_gripper_command(): cmd = Robotiq3FingerCommand() cmd.finger_a_command.speed = 0.5 diff --git a/victor_moveit_config/config/joint_limits.yaml b/victor_moveit_config/config/joint_limits.yaml index 5e5ee24f..9bbe121a 100644 --- a/victor_moveit_config/config/joint_limits.yaml +++ b/victor_moveit_config/config/joint_limits.yaml @@ -1,73 +1,76 @@ # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] + +# The max_velocity limits correspond to values in the Kuka controller joint_limits: victor_left_arm_joint_1: has_velocity_limits: true - max_velocity: 1 + max_velocity: 1.7453292519943295 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 1.1344640137963142 victor_left_arm_joint_2: has_velocity_limits: true - max_velocity: 1 + max_velocity: 1.7453292519943295 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 1.2217304763960306 victor_left_arm_joint_3: has_velocity_limits: true - max_velocity: 1 + max_velocity: 1.7453292519943295 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 8.726646259971647 victor_left_arm_joint_4: has_velocity_limits: true - max_velocity: 1 + max_velocity: 2.2689280275926285 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 8.726646259971647 victor_left_arm_joint_5: has_velocity_limits: true - max_velocity: 1 + max_velocity: 2.443460952792061 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 26.17993877991494 victor_left_arm_joint_6: has_velocity_limits: true - max_velocity: 1 + max_velocity: 3.141592653589793 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 20.943951023931955 victor_left_arm_joint_7: has_velocity_limits: true - max_velocity: 1 + max_velocity: 3.141592653589793 has_acceleration_limits: true + max_acceleration: 34.906585039886586 victor_right_arm_joint_1: has_velocity_limits: true - max_velocity: 1 + max_velocity: 1.7453292519943295 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 1.1344640137963142 victor_right_arm_joint_2: has_velocity_limits: true - max_velocity: 1 + max_velocity: 1.7453292519943295 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 1.2217304763960306 victor_right_arm_joint_3: has_velocity_limits: true - max_velocity: 1 + max_velocity: 1.7453292519943295 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 8.726646259971647 victor_right_arm_joint_4: has_velocity_limits: true - max_velocity: 1 + max_velocity: 2.2689280275926285 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 8.726646259971647 victor_right_arm_joint_5: has_velocity_limits: true - max_velocity: 1 + max_velocity: 2.443460952792061 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 26.17993877991494 victor_right_arm_joint_6: has_velocity_limits: true - max_velocity: 1 + max_velocity: 3.141592653589793 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 20.943951023931955 victor_right_arm_joint_7: has_velocity_limits: true - max_velocity: 1 + max_velocity: 3.141592653589793 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 34.906585039886586