def _start_ros_node_and_wait_for_sim(self): """ Starts the ros node, waits for sim to startup by waiting to get a message on the /joint_states channel :return: :rtype: """ print "sleeping for 5 seconds . . . " time.sleep(5.0) print "done sleeping" # Get the arm state to check sim is running rospy.init_node("iiwa_sim_test", anonymous=True) self._robotSubscriber = JointStateSubscriber("/joint_states") # wait for 5 seconds for robot movement service and /joint_states to come up # with more joints than just the gripper wait_time = 5 start_time = time.time() while (time.time() - start_time) < wait_time: if len(self._robotSubscriber.joint_timestamps.keys()) > 2: break print "Rostopic list: ", os.system("rostopic list") time.sleep(1) self.assertTrue( len(self._robotSubscriber.joint_timestamps.keys()) > 2, "Never received full robot joint positions on /joint_states topic")
def setupSubscribers(self): self.subscribers = dict() s = JointStateSubscriber(topic=self.config['joint_states_topic']) self.subscribers['joint_states'] = s
def do_main(): rospy.init_node('gamepad_teleop', anonymous=True) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) robotSubscriber = JointStateSubscriber("/joint_states") robotSubscriber = ros_utils.JointStateSubscriber("/joint_states") rospy.loginfo("Waiting for full kuka state...") while len(robotSubscriber.joint_positions.keys()) < 3: rospy.sleep(0.1) rospy.loginfo("got full state") rospy.loginfo("Grabbing controller...") pygame.init() pygame.joystick.init() joysticks = [ pygame.joystick.Joystick(x) for x in range(pygame.joystick.get_count()) ] rospy.loginfo("Found %d controllers" % len(joysticks)) if len(joysticks) == 0: rospy.logerr("Didn't find a controller :(.") sys.exit(-1) joystick = joysticks[0] joystick.init() rospy.loginfo("Using joystick %s" % joystick.get_name()) handDriver = SchunkDriver() gripper_goal_pos = 0.1 # Start by moving to an above-table pregrasp pose that we know # EE control will work well from (i.e. far from singularity) above_table_pre_grasp = [ 0.04486168762069299, 0.3256606458812486, -0.033502080520812445, -1.5769091802934694, 0.05899249087322813, 1.246379583616529, 0.38912999977004026 ] robotService = ros_utils.RobotService.makeKukaRobotService() success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=5) print("Moved to position") # Then kick off task space streaming sp = rospy.ServiceProxy('plan_runner/init_task_space_streaming', robot_msgs.srv.StartStreamingPlan) init = robot_msgs.srv.StartStreamingPlanRequest() init.force_guard.append(make_force_guard_msg(20.)) print sp(init) print("Started task space streaming") pub = rospy.Publisher('plan_runner/task_space_streaming_setpoint', robot_msgs.msg.CartesianGoalPoint, queue_size=1) tf_ee = None def cleanup(): rospy.wait_for_service("plan_runner/stop_plan") sp = rospy.ServiceProxy('plan_runner/stop_plan', std_srvs.srv.Trigger) init = std_srvs.srv.TriggerRequest() print sp(init) print("Done cleaning up and stopping streaming plan") frame_name = "iiwa_link_ee" # origin_tf, in the above EE frame origin_tf = transformations.euler_matrix(0.0, 0., 0.) origin_tf[0:3, 3] = np.array([0.15, 0.0, 0.0]) origin_tf_inv = np.linalg.inv(origin_tf) rospy.on_shutdown(cleanup) br = tf.TransformBroadcaster() try: last_gripper_update_time = time.time() last_update_time = time.time() while not rospy.is_shutdown(): #for axis_i in range(joystick.get_numaxes()): # rospy.loginfo("Axis %d: %f" % (axis_i, joystick.get_axis(axis_i))) #for button_i in range(joystick.get_numbuttons()): # rospy.loginfo("Button %d: %f" % (button_i, joystick.get_button(button_i))) #time.sleep(0.5) # Gamepad: Logitech 310 # DPad: Axis 1 +1 is down # Axis 0 +1 is right # Left stick: Axis 7 +1 is down # Axis 6 +1 is right # Right stick: Axis 3 +1 is right # Axis 4 +1 is down # Left bumper: Button 4 # Right bumper: Button 5 gripper_dt = time.time() - last_gripper_update_time dt = time.time() - last_update_time pygame.event.pump() if gripper_dt > 0.2: last_gripper_update_time = time.time() gripper_goal_pos += (joystick.get_button(5) - joystick.get_button(4)) * dt * 0.05 gripper_goal_pos = max(min(gripper_goal_pos, 0.1), 0.0) handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01) print "Gripper goal pos: ", gripper_goal_pos br.sendTransform( origin_tf[0:3, 3], ro(transformations.quaternion_from_matrix(origin_tf)), rospy.Time.now(), "origin_tf", frame_name) try: current_pose_ee = ros_utils.poseFromROSTransformMsg( tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print("Troubling looking up tf...") rate.sleep() continue if dt > 0.01: last_update_time = time.time() if tf_ee is None: tf_ee = tf_matrix_from_pose(current_pose_ee) tf_ee[0, 3] += -1. * joystick.get_axis(7) * dt * 0.25 tf_ee[1, 3] += -1. * joystick.get_axis(6) * dt * 0.25 tf_ee[2, 3] += -1. * joystick.get_axis(4) * dt * 0.25 dr = -1. * joystick.get_axis(0) * dt dp = 1. * joystick.get_axis(1) * dt dy = -1. * joystick.get_axis(3) * dt tf_ee = tf_ee.dot(transformations.euler_matrix(dr, 0., 0.)) tf_ee = tf_ee.dot(transformations.euler_matrix(0, dp, 0.)) tf_ee = tf_ee.dot(transformations.euler_matrix(0., 0., dy)) target_trans_ee = tf_ee[:3, 3] target_quat_ee = transformations.quaternion_from_matrix(tf_ee) new_msg = robot_msgs.msg.CartesianGoalPoint() new_msg.xyz_point.header.frame_id = "world" new_msg.xyz_point.point.x = target_trans_ee[0] new_msg.xyz_point.point.y = target_trans_ee[1] new_msg.xyz_point.point.z = target_trans_ee[2] new_msg.xyz_d_point.x = 0. new_msg.xyz_d_point.y = 0. new_msg.xyz_d_point.z = 0.0 new_msg.quaternion.w = target_quat_ee[0] new_msg.quaternion.x = target_quat_ee[1] new_msg.quaternion.y = target_quat_ee[2] new_msg.quaternion.z = target_quat_ee[3] new_msg.gain = make_cartesian_gains_msg(5., 10.) new_msg.ee_frame_id = frame_name pub.publish(new_msg) except Exception as e: print "Suffered exception ", e
frame_id="iiwa_link_ee", ee_frame_id="iiwa_link_ee") # TODO: deal with rotations not being right goal.gains.append(make_cartesian_gains_msg(0., 20.)) goal.force_guard.append(make_downward_force_guard_msg(25.)) return goal # Useful scanning positions central_view_pose = np.array([4.9, 13.7, -1.0, -95.5, -3.7, 51.4, 29.3 ]) * np.pi / 180. if __name__ == "__main__": rospy.init_node('carrot_collector', anonymous=True) robotSubscriber = JointStateSubscriber("/joint_states") configBundleSubscriber = rosUtils.SimpleSubscriber( "/carrot_configs", carrot_msgs.msg.CarrotConfigurationBundle) configBundleSubscriber.start() rospy.loginfo("Set up subscribers...") rospy.sleep(1.0) # EE CONTROL VERSION client = actionlib.SimpleActionClient( "plan_runner/CartesianTrajectory", robot_msgs.msg.CartesianTrajectoryAction) rospy.loginfo("Waiting for action servers...") client.wait_for_server() rospy.loginfo("Waiting for hearing full robot state...") while len(robotSubscriber.joint_positions.keys()) < 3:
class IiwaSimulationTest(unittest.TestCase): def setUp(self): self.kuka_joint_names = spartan_utils.get_kuka_joint_names() self._all_processes = [] self._terminate_all_processes() self._launch_procman_and_start_simulator() def tearDown(self): self._terminate_all_processes() pass def _terminate_all_processes(self): """ Kills all processes related to procman :return: :rtype: """ print "PRE KILLING TREE" os.system("pstree -ap") children = self._all_processes #get_children_pids(os.getpid()) for proc in children: proc.terminate() proc.wait() print "POST KILLING TREE:" os.system("pstree -ap") print "DONE WITH PRERUN CLEANUP" def _launch_procman_and_start_simulator(self): """ Launches procman, starts the simulation :return: :rtype: """ deputy = self._launch_process_and_test( ["/usr/bin/env", "bot-procman-deputy", "--name", "localhost"]) self._all_processes.append(deputy) sheriff = self._launch_process_and_test([ "/usr/bin/env", "bot-procman-sheriff", "--no-gui", "--on-script-complete", "exit", os.path.expandvars( "${SPARTAN_SOURCE_DIR}/apps/iiwa/iiwa_hardware.pmd"), "6.start_drake_iiwa_sim" ]) sheriff.wait() print "Sheriff returned code %d" % (sheriff.returncode) assert sheriff.returncode == 0, "Sheriff returned code %d" % ( sheriff.returncode) def _launch_process_and_test(self, args): """ Launch a process, assert that it has started up normally :param args: list of arguments, e.g. [executable, command line args, etc] :type args: list of strings :return: :rtype: """ p = subprocess.Popen(args) if p.poll() is not None: # Process launched and terminated. process_name = ''.join(args) assert p.returncode == 0, "Process %s returned with nonzero code %d" % ( process_name, p.returncode) return p def _start_ros_node_and_wait_for_sim(self): """ Starts the ros node, waits for sim to startup by waiting to get a message on the /joint_states channel :return: :rtype: """ print "sleeping for 5 seconds . . . " time.sleep(5.0) print "done sleeping" # Get the arm state to check sim is running rospy.init_node("iiwa_sim_test", anonymous=True) self._robotSubscriber = JointStateSubscriber("/joint_states") # wait for 5 seconds for robot movement service and /joint_states to come up # with more joints than just the gripper wait_time = 5 start_time = time.time() while (time.time() - start_time) < wait_time: if len(self._robotSubscriber.joint_timestamps.keys()) > 2: break print "Rostopic list: ", os.system("rostopic list") time.sleep(1) self.assertTrue( len(self._robotSubscriber.joint_timestamps.keys()) > 2, "Never received full robot joint positions on /joint_states topic") def setupTF(self): self.tfBuffer = tf2_ros.Buffer() self.tfListener = tf2_ros.TransformListener(self.tfBuffer) def get_ee_frame_pose(self): # check that desired position matches actual self.setupTF() ee_frame_name = "iiwa_link_ee" world_frame_name = "base" iiwa_link_ee_to_world = self.tfBuffer.lookup_transform( world_frame_name, ee_frame_name, rospy.Time(0), rospy.Duration(1)) return iiwa_link_ee_to_world def test_simulator_startup(self): """ Launches simulator, makes sure that it can ping roscore :return: :rtype: """ self._start_ros_node_and_wait_for_sim() def test_move_arm(self): """ Move the arm to a given position :return: :rtype: """ self._start_ros_node_and_wait_for_sim() # make robot service robotService = RobotService.makeKukaRobotService() targetPosition = [0.5] * 7 success = robotService.moveToJointPosition(targetPosition, timeout=5) self.assertTrue( success, msg="RobotService MoveToJointPosition returned failure ") # check that we actually reached the target position lastRobotJointPositions = self._robotSubscriber.get_position_vector_from_joint_names( self.kuka_joint_names) reached_target_position = np.linalg.norm( np.array(targetPosition) - np.array(lastRobotJointPositions[0:7])) < 0.1 if not reached_target_position: print "last robot joint position", lastRobotJointPositions self.assertTrue(reached_target_position, "Robot didn't reach target position") def test_ik_service(self): """ Test the ik service :return: :rtype: """ self._start_ros_node_and_wait_for_sim() above_table_pre_grasp = [ 0.04486168762069299, 0.3256606458812486, -0.033502080520812445, -1.5769091802934694, 0.05899249087322813, 1.246379583616529, 0.38912999977004026 ] poseStamped = geometry_msgs.msg.PoseStamped() pos = [0.51003723, 0.02411757, 0.30524811] quat = [0.68763689, 0.15390449, 0.69872774, -0.12348466] poseStamped.pose.position.x = pos[0] poseStamped.pose.position.y = pos[1] poseStamped.pose.position.z = pos[2] poseStamped.pose.orientation.w = quat[0] poseStamped.pose.orientation.x = quat[1] poseStamped.pose.orientation.y = quat[2] poseStamped.pose.orientation.z = quat[3] robotService = rosUtils.RobotService.makeKukaRobotService() response = robotService.runIK(poseStamped, seedPose=above_table_pre_grasp, nominalPose=above_table_pre_grasp) print "IK solution found ", response.success if response.success: print "moving to joint position", response.joint_state.position robotService.moveToJointPosition(response.joint_state.position) self.assertTrue(response.success) # check that desired position matches actual self.setupTF() ee_frame_name = "iiwa_link_ee" world_frame_name = "base" iiwa_link_ee_to_world = self.tfBuffer.lookup_transform( world_frame_name, ee_frame_name, rospy.Time(0), rospy.Duration(1)) pos_actual_xyz = iiwa_link_ee_to_world.transform.translation pos_actual = [0] * 3 pos_actual[0] = pos_actual_xyz.x pos_actual[1] = pos_actual_xyz.y pos_actual[2] = pos_actual_xyz.z eps = 0.01 pos_achieved = np.linalg.norm(np.array(pos) - np.array(pos_actual)) < eps self.assertTrue(pos_achieved, "Didn't achieve desired end-effector position") def test_cartesian_space_plan(self): """ Test the cartesian space plan """ self._start_ros_node_and_wait_for_sim() above_table_pre_grasp = [ 0.04486168762069299, 0.3256606458812486, -0.033502080520812445, -1.5769091802934694, 0.05899249087322813, 1.246379583616529, 0.38912999977004026 ] targetPosition = above_table_pre_grasp robotService = rosUtils.RobotService.makeKukaRobotService() success = robotService.moveToJointPosition(targetPosition, timeout=5) self.assertTrue( success, msg="RobotService MoveToJointPosition returned failure ") # check that we actually reached the target position lastRobotJointPositions = self._robotSubscriber.get_position_vector_from_joint_names( self.kuka_joint_names) reached_target_position = np.linalg.norm( np.array(targetPosition) - np.array(lastRobotJointPositions[0:7])) < 0.1 # now call the cartesian space plan service client = actionlib.SimpleActionClient( "plan_runner/CartesianTrajectory", robot_msgs.msg.CartesianTrajectoryAction) print "waiting for server" client.wait_for_server() print "connected to server" goal = make_cartesian_trajectory_goal_world_frame() goal.gains.append(make_cartesian_gains_msg()) goal.force_guard.append(make_force_guard_msg()) print "sending goal" client.send_goal(goal) rospy.loginfo("waiting for CartesianTrajectory action result") client.wait_for_result() result = client.get_result() rospy.sleep(3) # wait for controller to settle success = (result.status.status == robot_msgs.msg.PlanStatus.FINISHED_NORMALLY) print "result:", result self.assertTrue(success, msg="PlanStatus was not FINISHED_NORMALLY") # check the position # check that desired position matches actual self.setupTF() iiwa_link_ee_to_world = self.get_ee_frame_pose() pos_actual = np.array( rosUtils.pointMsgToList( iiwa_link_ee_to_world.transform.translation)) pos_desired = np.array( rosUtils.pointMsgToList(goal.trajectory.xyz_points[-1].point)) quat_actual = np.array( rosUtils.quatMsgToList(iiwa_link_ee_to_world.transform.rotation)) quat_desired = np.array( rosUtils.quatMsgToList(goal.trajectory.quaternions[0])) pos_tol = 0.01 # within 5 cm orientation_tol = 10 # within 5 degrees pos_error = np.linalg.norm(pos_actual - pos_desired) orientation_error_deg = 180 / np.pi * spartan_utils.compute_angle_between_quaternions( quat_actual, quat_desired) print "pos_error:\n", pos_error print "orientation error:\n", orientation_error_deg self.assertTrue(pos_error < pos_tol, msg="position error was above tolerance") self.assertTrue(orientation_error_deg < orientation_tol, msg="orientation error was above tolerance")
def do_main(): rospy.init_node('simple_teleop', anonymous=True) # setup listener for tf2s (used for ee and controller poses) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) robotSubscriber = JointStateSubscriber("/joint_states") # wait until robot state found robotSubscriber = ros_utils.JointStateSubscriber("/joint_states") print("Waiting for full kuka state...") while len(robotSubscriber.joint_positions.keys()) < 3: rospy.sleep(0.1) print("got full state") # init gripper handDriver = SchunkDriver() # init mouse manager mouse_manager = TeleopMouseManager() roll_goal = 0.0 yaw_goal = 0.0 pitch_goal = 0.0 # Start by moving to an above-table pregrasp pose that we know # EE control will work well from (i.e. far from singularity) stored_poses_dict = spartan_utils.getDictFromYamlFilename("../station_config/RLG_iiwa_1/stored_poses.yaml") above_table_pre_grasp = stored_poses_dict["Grasping"]["above_table_pre_grasp"] robotService = ros_utils.RobotService.makeKukaRobotService() success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=5) print("Moved to position") gripper_goal_pos = 0.0 handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01) print("sent close goal to gripper") time.sleep(2) gripper_goal_pos = 0.1 handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01) print("sent open goal to gripper") frame_name = "iiwa_link_ee" # end effector frame name for i in range(10): if i == 9: print "Couldn't find robot pose" sys.exit(0) try: ee_pose_above_table = ros_utils.poseFromROSTransformMsg( tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform) ee_tf_above_table = tf_matrix_from_pose(ee_pose_above_table) break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print("Troubling looking up robot pose...") time.sleep(0.1) # Then kick off task space streaming sp = rospy.ServiceProxy('plan_runner/init_task_space_streaming', robot_msgs.srv.StartStreamingPlan) init = robot_msgs.srv.StartStreamingPlanRequest() init.force_guard.append(make_force_guard_msg(20.)) res = sp(init) print("Started task space streaming") pub = rospy.Publisher('plan_runner/task_space_streaming_setpoint', robot_msgs.msg.CartesianGoalPoint, queue_size=1); def cleanup(): rospy.wait_for_service("plan_runner/stop_plan") sp = rospy.ServiceProxy('plan_runner/stop_plan', std_srvs.srv.Trigger) init = std_srvs.srv.TriggerRequest() print sp(init) print("Done cleaning up and stopping streaming plan") rospy.on_shutdown(cleanup) br = tf.TransformBroadcaster() rate = rospy.Rate(100) # max rate at which control should happen ee_tf_last_commanded = np.zeros((4,4)) def get_initial_pose(): while not rospy.is_shutdown(): # get current tf from ros world to ee try: ee_pose_current = ros_utils.poseFromROSTransformMsg( tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform) ee_tf_last_commanded = tf_matrix_from_pose(ee_pose_current) return ee_tf_last_commanded except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print("Troubling looking up robot pose...") rate.sleep() continue print ee_tf_last_commanded ee_tf_last_commanded = get_initial_pose() print ee_tf_last_commanded sys.path.append("../imitation_tools/scripts") from capture_imitation_data_client import start_bagging_imitation_data_client, stop_bagging_imitation_data_client start_bagging_imitation_data_client() pose_save_counter = 0 saved_pose_dict = dict() saved_pose_counter = 0 try: # control loop while not rospy.is_shutdown(): # # get teleop mouse events = mouse_manager.get_events() if events["r"]: print above_table_pre_grasp print "that was above_table_pre_grasp" success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=3) roll_goal = 0.0 yaw_goal = 0.0 pitch_goal = 0.0 ee_tf_last_commanded = get_initial_pose() continue pose_save_counter += 1 if events["o"] and pose_save_counter >= 100: # this make it not happen to much joint_name_list = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'] joint_position_vector = robotSubscriber.get_position_vector_from_joint_names(joint_name_list) print joint_position_vector print "joint positions saved" new_pose_name = "pose_"+str(saved_pose_counter).zfill(3) saved_pose_counter += 1 saved_pose_dict[new_pose_name] = joint_position_vector.tolist() pose_save_counter = 0 if events["escape"]: stop_bagging_imitation_data_client() if len(saved_pose_dict) > 0: spartan_utils.saveToYaml(saved_pose_dict, "saved_poses.yaml") sys.exit(0) scale_down = 0.0001 delta_x = events["delta_x"]*scale_down delta_y = events["delta_y"]*-scale_down delta_forward = 0.0 forward_scale = 0.001 if events["w"]: delta_forward -= forward_scale if events["s"]: delta_forward += forward_scale # extract and normalize quat from tf if events["rotate_left"]: roll_goal += 0.01 if events["rotate_right"]: roll_goal -= 0.01 roll_goal = np.clip(roll_goal, a_min = -0.9, a_max = 0.9) if events["side_button_back"]: yaw_goal += 0.01 print("side button back") if events["side_button_forward"]: yaw_goal -= 0.01 print("side side_button_forward") yaw_goal = np.clip(yaw_goal, a_min = -1.314, a_max = 1.314) if events["d"]: pitch_goal += 0.01 if events["a"]: pitch_goal -= 0.01 pitch_goal = np.clip(pitch_goal, a_min = -1.314, a_max = 1.314) R = transformations.euler_matrix(pitch_goal, roll_goal, yaw_goal, 'syxz') # third is "yaw", when in above table pre-grasp # second is "roll", '' # first must be "pitch" above_table_quat_ee = transformations.quaternion_from_matrix(R.dot(ee_tf_above_table)) above_table_quat_ee = np.array(above_table_quat_ee) / np.linalg.norm(above_table_quat_ee) # calculate controller position delta and add to start position to get target ee position target_translation = np.asarray([delta_forward, delta_x, delta_y]) empty_matrx = np.zeros_like(ee_tf_last_commanded) empty_matrx[:3, 3] = target_translation ee_tf_last_commanded += empty_matrx target_trans_ee = ee_tf_last_commanded[:3, 3] # publish target pose as cartesian goal point new_msg = robot_msgs.msg.CartesianGoalPoint() new_msg.xyz_point.header.stamp = rospy.Time.now() new_msg.xyz_point.header.frame_id = "world" new_msg.xyz_point.point.x = target_trans_ee[0] new_msg.xyz_point.point.y = target_trans_ee[1] new_msg.xyz_point.point.z = target_trans_ee[2] new_msg.xyz_d_point.x = 0.0 new_msg.xyz_d_point.y = 0.0 new_msg.xyz_d_point.z = 0.0 new_msg.quaternion.w = above_table_quat_ee[0] new_msg.quaternion.x = above_table_quat_ee[1] new_msg.quaternion.y = above_table_quat_ee[2] new_msg.quaternion.z = above_table_quat_ee[3] new_msg.roll = roll_goal new_msg.pitch = pitch_goal new_msg.yaw = yaw_goal new_msg.gain = make_cartesian_gains_msg(5., 10.) new_msg.ee_frame_id = frame_name pub.publish(new_msg) #gripper if events["mouse_wheel_up"]: gripper_goal_pos += 0.006 if events["mouse_wheel_down"]: gripper_goal_pos -= 0.006 if gripper_goal_pos < 0: gripper_goal_pos = 0.0 if gripper_goal_pos > 0.1: gripper_goal_pos = 0.1 handDriver.sendGripperCommand(gripper_goal_pos, speed=0.2, stream=True) rate.sleep() except Exception as e: print "Suffered exception ", e
traj_end = trajectory_msgs.msg.JointTrajectoryPoint() traj_end.positions = q + v traj_end.velocities = v traj_end.time_from_start = rospy.Duration(0.2) traj.points.append(traj_start) traj.points.append(traj_end) return traj if __name__ == "__main__": rospy.init_node('sandboxx') robotSubscriber = JointStateSubscriber("/joint_states") print("Waiting for full kuka state...") while len(robotSubscriber.joint_positions.keys()) < 3: rospy.sleep(0.1) print("got full state") print("Moving to start position") above_table_pre_grasp = [ 0.04486168762069299, 0.3256606458812486, -0.033502080520812445, -1.5769091802934694, 0.05899249087322813, 1.246379583616529, 0.38912999977004026 ] targetPosition = above_table_pre_grasp robotService = rosUtils.RobotService.makeKukaRobotService() success = robotService.moveToJointPosition(targetPosition, timeout=5)
def do_main(): rospy.init_node('sandbox', anonymous=True) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) robotSubscriber = JointStateSubscriber("/joint_states") robotSubscriber = ros_utils.JointStateSubscriber("/joint_states") print("Waiting for full kuka state...") while len(robotSubscriber.joint_positions.keys()) < 3: rospy.sleep(0.1) print("got full state") hydraSubscriber = SimpleSubscriber("/hydra_calib", razer_hydra.msg.Hydra) hydraSubscriber.start() print("Waiting for hydra startup...") hydraSubscriber.waitForNextMessage() print("Got hydra.") handDriver = SchunkDriver() gripper_goal_pos = 0.1 # Start by moving to an above-table pregrasp pose that we know # EE control will work well from (i.e. far from singularity) above_table_pre_grasp = [0.04486168762069299, 0.3256606458812486, -0.033502080520812445, -1.5769091802934694, 0.05899249087322813, 1.246379583616529, 0.38912999977004026] robotService = ros_utils.RobotService.makeKukaRobotService() success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=5) print("Moved to position") # Then kick off task space streaming sp = rospy.ServiceProxy('plan_runner/init_task_space_streaming', robot_msgs.srv.StartStreamingPlan) init = robot_msgs.srv.StartStreamingPlanRequest() init.force_guard.append(make_force_guard_msg(20.)) print sp(init) print("Started task space streaming") pub = rospy.Publisher('plan_runner/task_space_streaming_setpoint', robot_msgs.msg.CartesianGoalPoint, queue_size=1) # Control of robot works like this: # When the user is pressing no buttons on the # wand, nothing happens. # When the user depresses the top front trigger of # the wand (button index 0), the current wand position # is memorized as the origin, and any movement of the # wand from there is executed as incremental # movements of the end effector of the robot. # When the user releases the top front trigger, control # is ceased and pressing the button again re-zeros the robot. paddle_index = 0 enable_move_button_index = 0 enable_move_button_last_state = False start_pose_wand = None start_tf_ee = None #safe_space_bounds = np.array([ # [-0.1525049945545604, -0.29228035558728516, 0.13544803250537002], # [-0.4769166944956231, -0.35229338435348867, 0.19769445898112134], # [-0.457830238829753, 0.20935562473070765, 0.21958282208421853], # [-0.11156388045357436, 0.19961179929244402, 0.26618401754649285], # [-0.10225744036375771, 0.22933143089985508, 0.48444059628986263], # [-0.14861448807866284, -0.41030619355456643, 0.4648083304072826], # [-0.5242759491071456, -0.4147275210829423, 0.4948555294112139], # [-0.4847194053597296, 0.27176780719677074, 0.45391525596908033], # [-0.17358753502356636, 0.18660040610810102, 0.15775990744092278], # [-0.45109038331994794, 0.20434001341514574, 0.09804323148032473], # [-0.4716416007082929, -0.3620164988593509, 0.12965905105466402], # [-0.16130783846258154, -0.3208282816424661, 0.109649432666865]]) # Reasonable inner bounding box safe_space_lower = np.array([-0.45, -0.35, 0.125]) safe_space_upper = np.array([-0.15, 0.2, 0.45]) safe_space_violation = False last_safe_space_complaint = time.time() - 1000. def cleanup(): rospy.wait_for_service("plan_runner/stop_plan") sp = rospy.ServiceProxy('plan_runner/stop_plan', std_srvs.srv.Trigger) init = std_srvs.srv.TriggerRequest() print sp(init) print("Done cleaning up and stopping streaming plan") frame_name = "iiwa_link_ee" # origin_tf, in the above EE frame origin_tf = transformations.euler_matrix(0.0, 0., 0.) origin_tf[0:3, 3] = np.array([0.15, 0.0, 0.0]) origin_tf_inv = np.linalg.inv(origin_tf) rospy.on_shutdown(cleanup) br = tf.TransformBroadcaster() try: last_gripper_update_time = time.time() last_button_1_status = False while not rospy.is_shutdown(): latest_hydra_msg = hydraSubscriber.waitForNextMessage(sleep_duration=0.0001) dt = time.time() - last_gripper_update_time if dt > 0.2: last_gripper_update_time = time.time() gripper_goal_pos += latest_hydra_msg.paddles[0].joy[0]*dt*0.05 gripper_goal_pos = max(min(gripper_goal_pos, 0.1), 0.0) handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01) print "Gripper goal pos: ", gripper_goal_pos br.sendTransform(origin_tf[0:3, 3], ro(transformations.quaternion_from_matrix(origin_tf)), rospy.Time.now(), "origin_tf", frame_name) try: current_pose_ee = ros_utils.poseFromROSTransformMsg( tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print("Troubling looking up tf...") rate.sleep() continue # Check if the move button is press hydra_status = latest_hydra_msg.paddles[paddle_index] # Difference in tf from initial TF current_pose_wand = ros_utils.poseFromROSTransformMsg(hydra_status.transform) if hydra_status.buttons[1] and hydra_status.buttons[1] != last_button_1_status: print "Current wand pose: ", current_pose_wand wand_trans = np.array(current_pose_wand[0]) if np.any(wand_trans <= safe_space_lower) or np.any(wand_trans >= safe_space_upper): if time.time() - last_safe_space_complaint > 0.5: print "Safe space violation: ", wand_trans last_safe_space_complaint = time.time() safe_space_violation = True if hydra_status.buttons[enable_move_button_index] is False: enable_move_button_last_state = False safe_space_violation = False elif safe_space_violation is False: if enable_move_button_last_state is False: start_pose_wand = ros_utils.poseFromROSTransformMsg(hydra_status.transform) start_tf_wand = tf_matrix_from_pose(start_pose_wand) start_pose_ee = current_pose_ee start_tf_ee = tf_matrix_from_pose(start_pose_ee) enable_move_button_last_state = True last_button_1_status = hydra_status.buttons[1] # These expect quat in x y z w, rather than our normal w x y z br.sendTransform(start_pose_wand[0], ro(start_pose_wand[1]), rospy.Time.now(), "start_pose_wand", "base") br.sendTransform(current_pose_wand[0], ro(current_pose_wand[1]), rospy.Time.now(), "current_pose_wand", "base") br.sendTransform(start_pose_ee[0], ro(start_pose_ee[1]), rospy.Time.now(), "start_pose_ee", "base") hydra_tf = get_relative_tf_between_poses(start_pose_wand, current_pose_wand) # Target TF for the EE will be its start TF plus this offset rot_slerp_amount = 1.0 trans_slerp_amount = 1.0 tf_in_ee_frame = origin_tf.dot(hydra_tf).dot(origin_tf_inv) br.sendTransform(tf_in_ee_frame[0:3, 3], ro(transformations.quaternion_from_matrix(tf_in_ee_frame)), rospy.Time.now(), "tf_in_ee_frame", "base") start_tf_ee_inv = np.linalg.inv(start_tf_ee) target_tf_ee = start_tf_ee.copy() target_tf_ee[0:3, 3] += tf_in_ee_frame[0:3, 3] # copy position change in world frame target_tf_ee[0:3, 0:3] = tf_in_ee_frame[0:3, 0:3].dot(target_tf_ee[0:3, 0:3]) br.sendTransform(target_tf_ee[0:3, 3], ro(transformations.quaternion_from_matrix(target_tf_ee)), rospy.Time.now(), "target_tf_ee", "base") target_trans_ee = trans_slerp_amount*target_tf_ee[:3, 3] + (1. - trans_slerp_amount)*np.array(start_pose_ee[0]) target_quat_ee = transformations.quaternion_slerp( np.array(start_pose_ee[1]), transformations.quaternion_from_matrix(target_tf_ee), rot_slerp_amount) target_quat_ee = np.array(target_quat_ee) / np.linalg.norm(target_quat_ee) br.sendTransform(target_trans_ee, ro(target_quat_ee), rospy.Time.now(), "target_tf_ee_interp", "base") new_msg = robot_msgs.msg.CartesianGoalPoint() new_msg.xyz_point.header.frame_id = "world" new_msg.xyz_point.point.x = target_trans_ee[0] new_msg.xyz_point.point.y = target_trans_ee[1] new_msg.xyz_point.point.z = target_trans_ee[2] new_msg.xyz_d_point.x = 0. new_msg.xyz_d_point.y = 0. new_msg.xyz_d_point.z = 0.0 new_msg.quaternion.w = target_quat_ee[0] new_msg.quaternion.x = target_quat_ee[1] new_msg.quaternion.y = target_quat_ee[2] new_msg.quaternion.z = target_quat_ee[3] new_msg.gain = make_cartesian_gains_msg(5., 10.) new_msg.ee_frame_id = frame_name pub.publish(new_msg) except Exception as e: print "Suffered exception ", e