diff --git a/sr_error_reporter/src/UnderactuationErrorReporter.cpp b/sr_error_reporter/src/UnderactuationErrorReporter.cpp
index 716c93b5..9762013c 100644
--- a/sr_error_reporter/src/UnderactuationErrorReporter.cpp
+++ b/sr_error_reporter/src/UnderactuationErrorReporter.cpp
@@ -1,6 +1,6 @@
/*
* Software License Agreement (BSD License)
-* Copyright © 2021-2023 belongs to Shadow Robot Company Ltd.
+* Copyright © 2021-2024 belongs to Shadow Robot Company Ltd.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
@@ -39,11 +39,6 @@
UnderactuationErrorReporter::UnderactuationErrorReporter(ros::NodeHandle& node_handle)
: node_handle_(node_handle)
{
- trajectory_subscriber_left_ = node_handle.subscribe("/lh_trajectory_controller/state", 1,
- &UnderactuationErrorReporter::trajectory_callback_left, this, ros::TransportHints().tcpNoDelay());
- trajectory_subscriber_right_ = node_handle.subscribe("/rh_trajectory_controller/state", 1,
- &UnderactuationErrorReporter::trajectory_callback_right, this, ros::TransportHints().tcpNoDelay());
-
if (!wait_for_param(node_handle, "robot_description"))
{
ROS_ERROR("UnderactuationErrorReporter did't find robot_description on parameter server");
@@ -58,6 +53,11 @@ UnderactuationErrorReporter::UnderactuationErrorReporter(ros::NodeHandle& node_h
{
ROS_ERROR("UnderactuationErrorReporter did't find robot_description_semantic on parameter server");
}
+
+ trajectory_subscriber_left_ = node_handle.subscribe("/lh_trajectory_controller/state", 1,
+ &UnderactuationErrorReporter::trajectory_callback_left, this, ros::TransportHints().tcpNoDelay());
+ trajectory_subscriber_right_ = node_handle.subscribe("/rh_trajectory_controller/state", 1,
+ &UnderactuationErrorReporter::trajectory_callback_right, this, ros::TransportHints().tcpNoDelay());
}
ros::Publisher UnderactuationErrorReporter::get_or_create_publisher(
diff --git a/sr_multi_moveit/sr_multi_moveit_test/scripts/test_planners.py b/sr_multi_moveit/sr_multi_moveit_test/scripts/test_planners.py
index 109105d8..fbdfeccc 100755
--- a/sr_multi_moveit/sr_multi_moveit_test/scripts/test_planners.py
+++ b/sr_multi_moveit/sr_multi_moveit_test/scripts/test_planners.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
-# Copyright © 2019, 2022-2023 belongs to Shadow Robot Company Ltd.
+# Copyright © 2019, 2022-2024 belongs to Shadow Robot Company Ltd.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification,
@@ -267,8 +267,7 @@ def test_waypoints(self):
wpose = waypoints[0]
waypoints.append(copy.deepcopy(wpose))
- (plan3, fraction) = self.group.compute_cartesian_path(
- waypoints, 0.01, 0.0)
+ (plan3, fraction) = self.group.compute_cartesian_path(waypoints, 0.01)
if not self._check_plan(plan3) and fraction > 0.8:
self.fail_list.append("Failed: test_waypoints, " + self.planner)
else:
diff --git a/sr_robot_commander/src/sr_robot_commander/follow_warehouse_trajectory.py b/sr_robot_commander/src/sr_robot_commander/follow_warehouse_trajectory.py
index 14bcbc16..c7710f0c 100755
--- a/sr_robot_commander/src/sr_robot_commander/follow_warehouse_trajectory.py
+++ b/sr_robot_commander/src/sr_robot_commander/follow_warehouse_trajectory.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
-# Copyright © 2019, 2022-2023 belongs to Shadow Robot Company Ltd.
+# Copyright © 2019, 2022-2024 belongs to Shadow Robot Company Ltd.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification,
@@ -52,7 +52,6 @@ def __init__(self):
rospy.sleep(2)
group_id = rospy.get_param("~arm_group_name")
self.eef_step = rospy.get_param("~eef_step", 0.01)
- self.jump_threshold = rospy.get_param("~jump_threshold", 1000)
self.group = MoveGroupCommander(group_id)
self._robot_name = self.robot._r.get_robot_name()
@@ -151,8 +150,7 @@ def plan_from_list(self, waypoint_names):
# +1 because current position is used as first waypiont.
rospy.logerr("Not all waypoints existed, not executing.")
return False
- (plan, fraction) = self.group.compute_cartesian_path(
- waypoints, self.eef_step, self.jump_threshold)
+ (plan, fraction) = self.group.compute_cartesian_path(waypoints, self.eef_step)
if fraction < self._min_wp_fraction:
rospy.logerr("Only managed to generate trajectory through" +
diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
index 651c1f4e..ec21952a 100644
--- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
+++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
-# Copyright © 2015, 2022-2023 belongs to Shadow Robot Company Ltd.
+# Copyright © 2015, 2022-2024 belongs to Shadow Robot Company Ltd.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without modification,
@@ -808,15 +808,13 @@ def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self, waypoints, reference_frame=None,
- eef_step=0.005, jump_threshold=0.0, custom_start_state=None):
+ eef_step=0.005, custom_start_state=None):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given.
@param waypoints - an array of poses of end-effector.
@param eef_step - configurations are computed for every eef_step meters.
- @param jump_threshold - maximum distance in configuration space between consecutive points in the
- resulting path.
@param custom_start_state - specify a start state different than the current state.
@return - motion plan (RobotTrajectory msg) that contains the trajectory to the set wayapoints targets.
"""
@@ -827,7 +825,7 @@ def plan_to_waypoints_target(self, waypoints, reference_frame=None,
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
- self.__plan, fraction = self._move_group_commander.compute_cartesian_path(waypoints, eef_step, jump_threshold)
+ self.__plan, fraction = self._move_group_commander.compute_cartesian_path(waypoints, eef_step)
self.set_pose_reference_frame(old_frame)
return self.__plan, fraction
diff --git a/sr_robot_launch/launch/sr_ur_arm_hand.launch b/sr_robot_launch/launch/sr_ur_arm_hand.launch
index 819d3b5c..e64f729e 100644
--- a/sr_robot_launch/launch/sr_ur_arm_hand.launch
+++ b/sr_robot_launch/launch/sr_ur_arm_hand.launch
@@ -1,6 +1,6 @@
-
+
diff --git a/sr_robot_launch/launch/sr_ur_arms_hands.launch b/sr_robot_launch/launch/sr_ur_arms_hands.launch
index 510cf6e9..6932d0eb 100644
--- a/sr_robot_launch/launch/sr_ur_arms_hands.launch
+++ b/sr_robot_launch/launch/sr_ur_arms_hands.launch
@@ -1,6 +1,6 @@
-
+
diff --git a/sr_robot_launch/launch/srhand.launch b/sr_robot_launch/launch/srhand.launch
index 83386eac..6f67aa41 100644
--- a/sr_robot_launch/launch/srhand.launch
+++ b/sr_robot_launch/launch/srhand.launch
@@ -1,6 +1,6 @@
-
-
+
+
-
+
@@ -135,5 +135,5 @@
-
+