diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py
index b7f23a8..d949c85 100644
--- a/stretch_core/nodes/command_groups.py
+++ b/stretch_core/nodes/command_groups.py
@@ -347,8 +347,8 @@ class ArmCommandGroup(SimpleCommandGroup):
robot.arm.move_by(extension_error_m,
v_m=self.goal['velocity'],
a_m=self.goal['acceleration'],
- contact_thresh_pos_N=self.goal['contact_threshold'],
- contact_thresh_neg_N=-self.goal['contact_threshold'] \
+ contact_thresh_pos=self.goal['contact_threshold'],
+ contact_thresh_neg=-self.goal['contact_threshold'] \
if self.goal['contact_threshold'] is not None else None)
self.retracted = extension_error_m < 0.0
@@ -370,7 +370,7 @@ class ArmCommandGroup(SimpleCommandGroup):
arm_status = robot_status['arm']
arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0
pos = arm_status['pos'] + arm_backlash_correction
- return (pos, arm_status['vel'], arm_status['motor']['effort'])
+ return (pos, arm_status['vel'], arm_status['motor']['effort_pct'])
class LiftCommandGroup(SimpleCommandGroup):
@@ -392,8 +392,8 @@ class LiftCommandGroup(SimpleCommandGroup):
robot.lift.move_by(self.update_execution(robot_status)[1],
v_m=self.goal['velocity'],
a_m=self.goal['acceleration'],
- contact_thresh_pos_N=self.goal['contact_threshold'],
- contact_thresh_neg_N=-self.goal['contact_threshold'] \
+ contact_thresh_pos=self.goal['contact_threshold'],
+ contact_thresh_neg=-self.goal['contact_threshold'] \
if self.goal['contact_threshold'] is not None else None)
def update_execution(self, robot_status, **kwargs):
@@ -410,7 +410,7 @@ class LiftCommandGroup(SimpleCommandGroup):
def joint_state(self, robot_status, **kwargs):
lift_status = robot_status['lift']
- return (lift_status['pos'], lift_status['vel'], lift_status['motor']['effort'])
+ return (lift_status['pos'], lift_status['vel'], lift_status['motor']['effort_pct'])
class MobileBaseCommandGroup(SimpleCommandGroup):
@@ -556,17 +556,17 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
robot.base.translate_by(mobile_base_error_m,
v_m=self.goal_translate_mobile_base['velocity'],
a_m=self.goal_translate_mobile_base['acceleration'],
- contact_thresh_N=self.goal_translate_mobile_base['contact_threshold'])
+ contact_thresh=self.goal_translate_mobile_base['contact_threshold'])
elif mobile_base_error_rad is not None:
robot.base.rotate_by(mobile_base_error_rad,
v_r=self.goal_rotate_mobile_base['velocity'],
a_r=self.goal_rotate_mobile_base['acceleration'],
- contact_thresh_N=self.goal_rotate_mobile_base['contact_threshold'])
+ contact_thresh=self.goal_rotate_mobile_base['contact_threshold'])
else:
robot.base.translate_by(self.update_execution(robot_status)[1],
v_m=self.goal['velocity'],
a_m=self.goal['acceleration'],
- contact_thresh_N=self.goal['contact_threshold'])
+ contact_thresh=self.goal['contact_threshold'])
def update_execution(self, robot_status, **kwargs):
success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None
diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver
index 49cfa77..95f5c87 100755
--- a/stretch_core/nodes/stretch_driver
+++ b/stretch_core/nodes/stretch_driver
@@ -6,6 +6,7 @@ import threading
from rwlock import RWLock
import stretch_body.robot as rb
from stretch_body.hello_utils import ThreadServiceExit
+import stretch_body
import tf2_ros
import tf_conversions
@@ -338,6 +339,10 @@ class StretchBodyNode:
rospy.loginfo("{0} started".format(self.node_name))
+ if int(stretch_body.__version__.split('.')[1]) < 4:
+ rospy.logerr("ERROR: Found old stretch_body version. Please upgrade stretch_body to 0.4.0 or above.")
+ rospy.signal_shutdown('Found old stretch_body version.')
+
self.robot = rb.Robot()
self.robot.startup()
diff --git a/stretch_demos/nodes/clean_surface b/stretch_demos/nodes/clean_surface
index 7590fa9..55aedd9 100755
--- a/stretch_demos/nodes/clean_surface
+++ b/stretch_demos/nodes/clean_surface
@@ -135,8 +135,8 @@ class CleanSurfaceNode(hm.HelloNode):
self.lower_tool_until_contact()
else:
lift_m = self.lift_position - (above_surface_m - 0.02)
- lift_contact_effort = 20.0 #20.0 from funmap
- extension_contact_effort = 40.0 #40.0 from funmap
+ lift_contact_effort = 33.7 #effort_pct #20.0 from funmap
+ extension_contact_effort = 16.45 #effort_pct #40.0 from funmap
pose = {'joint_lift': (lift_m, lift_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True)
diff --git a/stretch_demos/nodes/hello_world b/stretch_demos/nodes/hello_world
index cde2ab4..34bf832 100755
--- a/stretch_demos/nodes/hello_world
+++ b/stretch_demos/nodes/hello_world
@@ -69,7 +69,7 @@ class HelloWorldNode(hm.HelloNode):
wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states)
extension_m = wrist_position + max_reach_m
extension_m = min(extension_m, max_extension_m)
- extension_contact_effort = 45.0
+ extension_contact_effort = 18.5 #effort_pct
pose = {'wrist_extension': (extension_m, extension_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True)
@@ -83,7 +83,7 @@ class HelloWorldNode(hm.HelloNode):
extension_m = wrist_position - backoff_m
extension_m = min(extension_m, max_extension_m)
extension_m = max(min_extension_m, extension_m)
- extension_contact_effort = 80.0 # to avoid stopping due to contact
+ extension_contact_effort = 32.9 #effort_pct # to avoid stopping due to contact
pose = {'wrist_extension': (extension_m, extension_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True)
rospy.sleep(1.0) # Give it time to backoff before the next move. Otherwise it may not backoff.
diff --git a/stretch_demos/nodes/open_drawer b/stretch_demos/nodes/open_drawer
index 6ecc2b2..d2a97c0 100755
--- a/stretch_demos/nodes/open_drawer
+++ b/stretch_demos/nodes/open_drawer
@@ -24,6 +24,7 @@ import argparse as ap
import hello_helpers.hello_misc as hm
import stretch_funmap.navigate as nv
+from stretch_body.robot_params import RobotParams
class OpenDrawerNode(hm.HelloNode):
@@ -36,18 +37,18 @@ class OpenDrawerNode(hm.HelloNode):
self.letter_height_m = 0.2
self.wrist_position = None
self.lift_position = None
-
+
def joint_states_callback(self, joint_states):
- with self.joint_states_lock:
+ with self.joint_states_lock:
self.joint_states = joint_states
wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(joint_states)
- self.wrist_position = wrist_position
+ self.wrist_position = wrist_position
lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states)
self.lift_position = lift_position
-
+
def align_to_surface(self):
rospy.loginfo('align_to_surface')
- trigger_request = TriggerRequest()
+ trigger_request = TriggerRequest()
trigger_result = self.trigger_align_with_nearest_cliff_service(trigger_request)
rospy.loginfo('trigger_result = {0}'.format(trigger_result))
@@ -57,15 +58,17 @@ class OpenDrawerNode(hm.HelloNode):
max_reach_m = 0.4
extension_m = self.wrist_position + max_reach_m
extension_m = min(extension_m, max_extension_m)
- extension_contact_effort = 45.0 #42.0 #40.0 from funmap
+ extension_contact_effort = 40 #effort_pct #42.0 #40.0 from funmap
+ if RobotParams.get_params()[1]['robot']['model_name']=='RE1V0':
+ extension_contact_effort = 18.5
pose = {'wrist_extension': (extension_m, extension_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True)
-
+
def lower_hook_until_contact(self):
rospy.loginfo('lower_hook_until_contact')
max_drop_m = 0.15
lift_m = self.lift_position - max_drop_m
- lift_contact_effort = 16.0 #18.0 #20.0 #20.0 from funmap
+ lift_contact_effort = 32.5 #effort_pct #18.0 #20.0 #20.0 from funmap
pose = {'joint_lift': (lift_m, lift_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True)
@@ -82,7 +85,7 @@ class OpenDrawerNode(hm.HelloNode):
rospy.loginfo('raise_hook_until_contact')
max_raise_m = 0.15
lift_m = self.lift_position + max_raise_m
- lift_contact_effort = 45.0
+ lift_contact_effort = 41.4 #effort_pct
pose = {'joint_lift': (lift_m, lift_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True)
@@ -94,7 +97,7 @@ class OpenDrawerNode(hm.HelloNode):
pose = {'joint_lift': lift_m}
self.move_to_pose(pose)
rospy.sleep(0.5) # wait for new lift position
-
+
def backoff_from_surface(self):
rospy.loginfo('backoff_from_surface')
if self.wrist_position is not None:
@@ -113,7 +116,7 @@ class OpenDrawerNode(hm.HelloNode):
extension_m = self.wrist_position - 0.2
extension_m = min(extension_m, max_extension_m)
extension_m = max(0.01, extension_m)
- extension_contact_effort = 120 #100.0 #40.0 from funmap
+ extension_contact_effort = 64.4 #effort_pct #100.0 #40.0 from funmap
pose = {'wrist_extension': (extension_m, extension_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True)
return True
@@ -131,7 +134,7 @@ class OpenDrawerNode(hm.HelloNode):
else:
rospy.logerr('pull_open: self.wrist_position is None!')
return False
-
+
def move_to_initial_configuration(self):
initial_pose = {'wrist_extension': 0.01,
'joint_wrist_yaw': 1.570796327,
@@ -141,7 +144,7 @@ class OpenDrawerNode(hm.HelloNode):
def trigger_open_drawer_down_callback(self, request):
return self.open_drawer('down')
-
+
def trigger_open_drawer_up_callback(self, request):
return self.open_drawer('up')
@@ -158,11 +161,11 @@ class OpenDrawerNode(hm.HelloNode):
message='Failed to backoff from the surface.'
)
- if direction == 'down':
+ if direction == 'down':
self.lower_hook_until_contact()
elif direction == 'up':
self.raise_hook_until_contact()
-
+
success = self.pull_open()
if not success:
return TriggerResponse(
@@ -171,21 +174,21 @@ class OpenDrawerNode(hm.HelloNode):
)
push_drawer_closed = False
- if push_drawer_closed:
+ if push_drawer_closed:
rospy.sleep(3.0)
self.push_closed()
-
+
return TriggerResponse(
success=True,
message='Completed opening the drawer!'
)
-
+
def main(self):
hm.HelloNode.main(self, 'open_drawer', 'open_drawer', wait_for_first_pointcloud=False)
self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
-
+
self.trigger_open_drawer_service = rospy.Service('/open_drawer/trigger_open_drawer_down',
Trigger,
self.trigger_open_drawer_down_callback)
@@ -193,8 +196,8 @@ class OpenDrawerNode(hm.HelloNode):
self.trigger_open_drawer_service = rospy.Service('/open_drawer/trigger_open_drawer_up',
Trigger,
self.trigger_open_drawer_up_callback)
-
-
+
+
rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff')
rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.')
self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy('/funmap/trigger_align_with_nearest_cliff', Trigger)
@@ -206,12 +209,12 @@ class OpenDrawerNode(hm.HelloNode):
rospy.wait_for_service('/funmap/trigger_lower_until_contact')
rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)
-
+
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
rate.sleep()
-
+
if __name__ == '__main__':
try:
parser = ap.ArgumentParser(description='Open Drawer behavior for stretch.')
diff --git a/stretch_description/package.xml b/stretch_description/package.xml
index b1a9403..9b59ba3 100644
--- a/stretch_description/package.xml
+++ b/stretch_description/package.xml
@@ -63,6 +63,7 @@
robot_state_publisher
rviz
+ rviz_imu_plugin
joint_state_publisher