diff --git a/franka_control_wrappers/ros_nodes/set_panda_defaults.py b/franka_control_wrappers/ros_nodes/set_panda_defaults.py index 91ccb4d..5699d22 100755 --- a/franka_control_wrappers/ros_nodes/set_panda_defaults.py +++ b/franka_control_wrappers/ros_nodes/set_panda_defaults.py @@ -25,9 +25,16 @@ rospy.wait_for_service('/franka_control/set_EE_frame') eef_srv = rospy.ServiceProxy('/franka_control/set_EE_frame', SetEEFrame) eef_msg = SetEEFrameRequest() -# This is the default settings + 35mm down for the gripper -gripper_offset = 0.035 -eef_msg.F_T_EE = [0.707099974155426, -0.707099974155426, 0.0, 0.0, 0.707099974155426, 0.707099974155426, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, gripper_offset + 0.10339999943971634, 1.0] + +# franka_msgs.msg.FrankaState keeps track of the updated F_T_EE, so the default transform has to be hardcoded +default_F_T_EE = [0.7071, -0.7071, 0.0, 0.0, 0.7071, 0.7071, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1034, 1.0] + +# Add the configured gripper height to the z-axis in the translation vector part of the transform +gripper_offset = rospy.get_param("/panda_setup/camera_mount_extra_height", 0.035) + +current_F_T_EE = default_F_T_EE +current_F_T_EE[14] += gripper_offset +eef_msg.F_T_EE = current_F_T_EE res = eef_srv.call(eef_msg).success if not res: diff --git a/franka_control_wrappers/src/franka_control_wrappers/panda_commander.py b/franka_control_wrappers/src/franka_control_wrappers/panda_commander.py index 6abe106..0a4cc01 100644 --- a/franka_control_wrappers/src/franka_control_wrappers/panda_commander.py +++ b/franka_control_wrappers/src/franka_control_wrappers/panda_commander.py @@ -23,6 +23,20 @@ def __init__(self, group_name=None): self.reset_publisher = rospy.Publisher('/franka_control/error_recovery/goal', ErrorRecoveryActionGoal, queue_size=1) + def set_named_poses(self): + """ + Fetch poses used in the application via their name from the parameter server and store them in the active move_group + """ + # 1 2 3 4 5 6 7 + grip_ready_joint_values_as_dict = rospy.get_param("/panda_setup/remember_joint_values/grip_ready", [-0.1706, -0.8882, -0.8514, -1.9803, -0.6403, 1.3514, 0.6152]) + grip_ready_joint_values = [v for k, v in sorted(grip_ready_joint_values_as_dict.items())] + self.active_group.remember_joint_values('grip_ready', grip_ready_joint_values) + + # 1 2 3 4 5 6 7 + drop_box_joint_values_as_dict = rospy.get_param("/panda_setup/remember_joint_values/drop_box", [ 0.8263, 1.0896, 0.8203, -0.8825, -0.7649, 1.7213, 2.2823]) + drop_box_joint_values = [v for k, v in sorted(drop_box_joint_values_as_dict.items())] + self.active_group.remember_joint_values('drop_box', drop_box_joint_values) + def print_debug_info(self): if self.active_group: planning_frame = self.active_group.get_planning_frame() @@ -36,6 +50,9 @@ def print_debug_info(self): print(self.robot.get_current_state()) print("") + def get_current_pose(self): + return self.active_group.get_current_pose().pose + def set_group(self, group_name): """ Set the active move group diff --git a/ggcnn/src/ggcnn/ggcnn.py b/ggcnn/src/ggcnn/ggcnn.py index 7898030..924a286 100644 --- a/ggcnn/src/ggcnn/ggcnn.py +++ b/ggcnn/src/ggcnn/ggcnn.py @@ -6,14 +6,14 @@ import tensorflow as tf from tensorflow.keras.models import load_model -from tensorflow.keras.backend import set_session +from tensorflow.compat.v1.keras.backend import set_session from dougsm_helpers.timeit import TimeIt MODEL_FILE = 'models/epoch_29_model.hdf5' -sess = tf.Session() +sess = tf.compat.v1.Session() set_session(sess) -graph = tf.get_default_graph() +graph = tf.compat.v1.get_default_graph() model = load_model(path.join(path.dirname(__file__), MODEL_FILE)) diff --git a/mvp_grasping/cfg/mvp_grasp.yaml b/mvp_grasping/cfg/mvp_grasp.yaml index 7617a7e..9983a63 100644 --- a/mvp_grasping/cfg/mvp_grasp.yaml +++ b/mvp_grasping/cfg/mvp_grasp.yaml @@ -12,7 +12,7 @@ grasp_entropy_node: cost: dist_from_best_scale: 0.15 dist_from_best_gain: 0.2 - dist_from_prev_view_scale: 0.0 + dist_from_prev_view_scale: 1.0 dist_from_prev_view_gain: 0.0 height: z1: 0.55 diff --git a/mvp_grasping/cfg/panda_setup.yaml b/mvp_grasping/cfg/panda_setup.yaml new file mode 100644 index 0000000..c7bfc1b --- /dev/null +++ b/mvp_grasping/cfg/panda_setup.yaml @@ -0,0 +1,18 @@ +camera_mount_extra_height: 0.035 +remember_joint_values: + grip_ready: + panda_joint1: -0.1706 + panda_joint2: -0.8882 + panda_joint3: -0.8514 + panda_joint4: -1.9803 + panda_joint5: -0.6403 + panda_joint6: 1.3514 + panda_joint7: 0.6152 + drop_box: + panda_joint1: 0.8263 + panda_joint2: 1.0896 + panda_joint3: 0.8203 + panda_joint4: -0.8825 + panda_joint5: -0.7649 + panda_joint6: -1.7213 + panda_joint7: 2.2823 diff --git a/mvp_grasping/launch/grasp_entropy_service.launch b/mvp_grasping/launch/grasp_entropy_service.launch index cdd52b0..53a5fcc 100644 --- a/mvp_grasping/launch/grasp_entropy_service.launch +++ b/mvp_grasping/launch/grasp_entropy_service.launch @@ -1,4 +1,5 @@ - + + diff --git a/mvp_grasping/launch/robot_bringup.launch b/mvp_grasping/launch/robot_bringup.launch index ec09e87..25e8414 100644 --- a/mvp_grasping/launch/robot_bringup.launch +++ b/mvp_grasping/launch/robot_bringup.launch @@ -1,21 +1,21 @@ - + + - - + + - - - + + + + + + - - - - diff --git a/mvp_grasping/launch/wrist_realsense.launch b/mvp_grasping/launch/wrist_realsense.launch index d364ea6..ba5e69b 100644 --- a/mvp_grasping/launch/wrist_realsense.launch +++ b/mvp_grasping/launch/wrist_realsense.launch @@ -1,202 +1,13 @@ - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + diff --git a/mvp_grasping/ros_nodes/grasp_entropy_node.py b/mvp_grasping/ros_nodes/grasp_entropy_node.py index 1357398..bf76e18 100755 --- a/mvp_grasping/ros_nodes/grasp_entropy_node.py +++ b/mvp_grasping/ros_nodes/grasp_entropy_node.py @@ -95,7 +95,7 @@ def update_service_handler(self, req): """ Update the GridWorld with a new observation, compute the viewpoint entropy and generate a new command. :param req: Ignored - :return: NextViewpointResponse (success flag, best grsap, velocity command) + :return: NextViewpointResponse (success flag, best grasp, velocity command) """ # Some initial checks diff --git a/mvp_grasping/ros_nodes/panda_mvp_grasp.py b/mvp_grasping/ros_nodes/panda_mvp_grasp.py old mode 100644 new mode 100755 index 7611091..196c9bc --- a/mvp_grasping/ros_nodes/panda_mvp_grasp.py +++ b/mvp_grasping/ros_nodes/panda_mvp_grasp.py @@ -17,6 +17,9 @@ def __init__(self): super(MVPGraspController, self).__init__() self.update_rate = 10.0 # Hz + def trigger_update(self): + super(MVPGraspController, self).trigger_update() + def __velo_control_loop(self): """ Perform velocity control using the MVP controller. @@ -36,7 +39,7 @@ def __velo_control_loop(self): ctr += 1 if ctr >= self.curr_velocity_publish_rate/self.update_rate: ctr = 0 - self.__trigger_update() + self.trigger_update() # Cartesian Contact if any(self.robot_state.cartesian_contact): @@ -56,6 +59,32 @@ def __velo_control_loop(self): return not rospy.is_shutdown() + def setup_velocity_control_loop(self): + super(MVPGraspController, self).setup_velocity_control_loop() + + def execute_velocity_control_loop(self): + """Perform the velocity control portion.""" + self._in_velo_loop = True + velo_ok = self.__velo_control_loop() + self._in_velo_loop = False + return velo_ok + + + def execute_best_grasp(self): + super(MVPGraspController, self).execute_best_grasp() + + + def release_object(self): + super(MVPGraspController, self).release_object() + + + def check_success_using_scales(self, run): + super(MVPGraspController, self).check_success_using_scales(run) + + def go(self): + super(MVPGraspController, self).go() + + if __name__ == '__main__': rospy.init_node('ap_grasping_velo') agc = MVPGraspController() diff --git a/mvp_grasping/src/mvp_grasping/panda_base_grasping_controller.py b/mvp_grasping/src/mvp_grasping/panda_base_grasping_controller.py index 0aa091e..8ed22a9 100644 --- a/mvp_grasping/src/mvp_grasping/panda_base_grasping_controller.py +++ b/mvp_grasping/src/mvp_grasping/panda_base_grasping_controller.py @@ -29,6 +29,10 @@ def __init__(self, output_desc='run', output_dir='~'): dt = datetime.datetime.now().strftime('%m%d_%H%M%S') self.out_file = os.path.join(output_dir, '%s_%s.txt' % (dt, output_desc)) + dirname = os.path.dirname(self.out_file) + if not os.path.exists(dirname): + os.makedirs(dirname) + def write_line(self, l): with open(self.out_file, 'a') as f: f.write(l) @@ -76,7 +80,7 @@ def __init__(self, experiment): self.t0 = 0 self.t1 = 0 self.success = False - self.qualtiy = None + self.quality = None self.entropy = None def start(self): @@ -139,7 +143,7 @@ def log_run(self, run): class BaseGraspController(object): """ - An base class for performing grasping experiments with the Panda robot. + A base class for performing grasping experiments with the Panda robot. """ def __init__(self): entropy_node_name = '/grasp_entropy_node' @@ -168,6 +172,7 @@ def __init__(self): 'velocity': 'cartesian_velocity_node_controller'}) self.cs.switch_controller('moveit') self.pc = PandaCommander(group_name='panda_arm_hand') + self.pc.set_named_poses() self.robot_state = None self.ROBOT_ERROR_DETECTED = False @@ -241,7 +246,7 @@ def __update_callback(self, msg): tfh.publish_pose_as_transform(self.best_grasp.pose, 'panda_link0', 'G', 0.05) - def __trigger_update(self): + def trigger_update(self): # Let ROS handle the threading for me. self.update_pub.publish(Empty()) @@ -253,11 +258,11 @@ def __execute_best_grasp(self): # Offset for initial pose. initial_offset = 0.05 - LINK_EE_OFFSET = 0.138 + LINK_EE_OFFSET = self.robot_state.F_T_EE[14] # Add some limits, plus a starting offset. self.best_grasp.pose.position.z = max(self.best_grasp.pose.position.z - 0.01, 0.026) # 0.021 = collision with ground - self.best_grasp.pose.position.z += initial_offset + LINK_EE_OFFSET # Offset from end efector position to + self.best_grasp.pose.position.z += initial_offset + LINK_EE_OFFSET # Offset from end effector position to self.pc.set_gripper(self.best_grasp.width, wait=False) rospy.sleep(0.1) @@ -296,30 +301,69 @@ def stop(self): self.curr_velo = Twist() self.curr_velo_pub.publish(self.curr_velo) - def go(self): - raw_input('Press Enter to Start.') - while not rospy.is_shutdown(): - self.cs.switch_controller('moveit') - self.pc.goto_named_pose('grip_ready', velocity=0.25) - start_pose = list(self.pregrasp_pose) - start_pose[0] += np.random.randn() * 0.05 - start_pose[1] += np.random.randn() * 0.05 - self.pc.goto_pose(start_pose, velocity=0.25) - self.pc.set_gripper(0.1) - self.cs.switch_controller('velocity') + def setup_velocity_control_loop(self): + self.cs.switch_controller('moveit') + self.pc.goto_named_pose('grip_ready', velocity=0.25) + start_pose = self.pc.get_current_pose() + start_pose.position.x += np.random.randn() * 0.05 + start_pose.position.y += np.random.randn() * 0.05 + self.pc.goto_pose(start_pose, velocity=0.25) + self.pc.set_gripper(0.1) + self.cs.switch_controller('velocity') + self.entropy_reset_srv.call() + + def execute_velocity_control_loop(self): + """Perform the velocity control portion.""" + self._in_velo_loop = True + velo_ok = self.__velo_control_loop() + self._in_velo_loop = False + return velo_ok + + + def execute_best_grasp(self): + """Execute the Grasp""" + grasp_ret = self.__execute_best_grasp() + return grasp_ret - self.entropy_reset_srv.call() - self.__trigger_update() + def release_object(self): + """Release Object""" + self.cs.switch_controller('moveit') + self.pc.goto_named_pose('grip_ready', velocity=0.5) + self.pc.goto_named_pose('drop_box', velocity=0.5) + self.pc.set_gripper(0.1) + + + def check_success_using_scales(self, run): + """Check success using the scales""" + rospy.sleep(1.0) + grasp_success = self.__weight_increase_check() + if not grasp_success: + rospy.logerr("Failed Grasp") + m = AddFailurePointRequest() + m.point.x = self.best_grasp.pose.position.x + m.point.y = self.best_grasp.pose.position.y + self.add_failure_point_srv.call(m) + else: + rospy.logerr("Successful Grasp") + run.success = grasp_success + run.quality = self.best_grasp.quality + run.entropy = self.best_grasp.entropy + run.viewpoints = self.viewpoints + run.save() + + def go(self): + raw_input('Press Enter to Start.') + while not rospy.is_shutdown(): + self.setup_velocity_control_loop() + + self.trigger_update() + run = self.experiment.new_run() run.start() - - # Perform the velocity control portion. - self._in_velo_loop = True - velo_ok = self.__velo_control_loop() - self._in_velo_loop = False - if not velo_ok: + + if not self.execute_velocity_control_loop(): rospy.sleep(1.0) if self.BAD_UPDATE: raw_input('Fix Me! Enter to Continue') @@ -329,8 +373,7 @@ def go(self): rospy.logerr('Aborting this Run.') continue - # Execute the Grasp - grasp_ret = self.__execute_best_grasp() + grasp_ret = self.execute_best_grasp() run.stop() if not grasp_ret or self.ROBOT_ERROR_DETECTED: @@ -339,27 +382,6 @@ def go(self): self.__recover_robot_from_error() continue - # Release Object - self.cs.switch_controller('moveit') - self.pc.goto_named_pose('grip_ready', velocity=0.5) - self.pc.goto_named_pose('drop_box', velocity=0.5) - self.pc.set_gripper(0.1) - - # Check success using the scales. - rospy.sleep(1.0) - grasp_success = self.__weight_increase_check() - if not grasp_success: - rospy.logerr("Failed Grasp") - m = AddFailurePointRequest() - m.point.x = self.best_grasp.pose.position.x - m.point.y = self.best_grasp.pose.position.y - self.add_failure_point_srv.call(m) - else: - rospy.logerr("Successful Grasp") - - run.success = grasp_success - run.quality = self.best_grasp.quality - run.entropy = self.best_grasp.entropy - run.viewpoints = self.viewpoints - run.save() + self.release_object() + self.check_success_using_scales(run) diff --git a/requirements.txt b/requirements.txt index 19964d8..e934790 100644 --- a/requirements.txt +++ b/requirements.txt @@ -5,4 +5,4 @@ opencv-python==4.0.0.21 Pillow==6.2.2 pybullet==2.4.8 scikit-image==0.14.5 -tensorflow-gpu==1.14.0 +tensorflow-auto-detect==1.14.0