def setupSubscribers(self): self.subscribers = dict() s = SimpleSubscriber(self.config['joint_states_topic'], sensor_msgs.msg.JointState) s.start() self.subscribers['joint_states'] = s
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 3 seconds . . . " time.sleep(3.0) print "done sleeping" # Get the arm state to check sim is running rospy.init_node("iiwa_sim_test", anonymous=True) self._robotSubscriber = SimpleSubscriber("/joint_states", sensor_msgs.msg.JointState) self._robotSubscriber.start() # wait for 5 seconds for robot movement service and /joint_states to come up wait_time = 5 start_time = time.time() while (time.time() - start_time) < wait_time: if self._robotSubscriber.hasNewMessage: break time.sleep(1) self.assertTrue( self._robotSubscriber.hasNewMessage, "Never received robot joint positions on /joint_states topic")
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
class IiwaSimulationTest(unittest.TestCase): def setUp(self): self._kill_all_procman_processes() self._all_processes = [] self._launch_procman_and_start_simulator() def tearDown(self): self._kill_all_processes() self._kill_all_procman_processes() pass def _kill_all_procman_processes(self): """ Kills all processes related to procman :return: :rtype: """ cmd = ["pkill", "-f", "procman"] process = subprocess.call(cmd) 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"]) 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) # print "sleeping for 3 seconds . . . " # time.sleep(3.0) # print "done sleeping" # # # Get the arm state to check sim is running # rospy.init_node("iiwa_sim_test", anonymous=True) # self._robotSubscriber = SimpleSubscriber("/joint_states", sensor_msgs.msg.JointState) # self._robotSubscriber.start() # # # wait for 5 seconds for robot movement service and /joint_states to come up # wait_time = 5 # start_time = time.time() # while (time.time() - start_time) < wait_time: # if self._robotSubscriber.hasNewMessage: # break # time.sleep(1) # # assert self._robotSubscriber.hasNewMessage, "Never received robot joint positions on /joint_states topic" 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) self._all_processes.append(p) return p def _kill_all_processes(self, additional_processes=None): """ Kill all processes in self._all_processes Also kill any additional processes that may have been passed in :param additional_processes: :type additional_processes: :return: :rtype: """ for p in self._all_processes: if p.poll() is None: p.kill() if additional_processes is None: additional_processes = [] for p in additional_processes: if p.poll() is None: p.kill() 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 3 seconds . . . " time.sleep(3.0) print "done sleeping" # Get the arm state to check sim is running rospy.init_node("iiwa_sim_test", anonymous=True) self._robotSubscriber = SimpleSubscriber("/joint_states", sensor_msgs.msg.JointState) self._robotSubscriber.start() # wait for 5 seconds for robot movement service and /joint_states to come up wait_time = 5 start_time = time.time() while (time.time() - start_time) < wait_time: if self._robotSubscriber.hasNewMessage: break time.sleep(1) self.assertTrue( self._robotSubscriber.hasNewMessage, "Never received robot joint positions on /joint_states topic") def setupTF(self): self.tfBuffer = tf2_ros.Buffer() self.tfListener = tf2_ros.TransformListener(self.tfBuffer) 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.lastMsg.position 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")
class IiwaSimulationTest(unittest.TestCase): def setUp(self): 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 = SimpleSubscriber("/joint_states", sensor_msgs.msg.JointState) self._robotSubscriber.start() # wait for 5 seconds for robot movement service and /joint_states to come up wait_time = 5 start_time = time.time() while (time.time() - start_time) < wait_time: if self._robotSubscriber.hasNewMessage: break print "Rostopic list: ", os.system("rostopic list") time.sleep(1) self.assertTrue(self._robotSubscriber.hasNewMessage, "Never received 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.lastMsg.position 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.lastMsg.position 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 try_to_move_arm_and_hand(): import rospy import rosgraph import socket import numpy as np import time try: rosgraph.Master('/rostopic').getPid() except socket.error: print "Unable to communicate with ROS master, even though it should be up!" cleanup_and_exit(1) rospy.init_node("test_full_simulation_stack", anonymous=True) # Get the arm state from spartan.utils.ros_utils import SimpleSubscriber robotSubscriber = SimpleSubscriber("/joint_states", sensor_msgs.msg.JointState) robotSubscriber.start() # Move the arm from spartan.utils.ros_utils import RobotService robotService = RobotService.makeKukaRobotService() targetPosition = [0.5] * 7 success = robotService.moveToJointPosition(targetPosition, timeout=5) if not success: print "RobotService MoveToJointPosition returned failure ", success rospy.sleep(0.1) if robotSubscriber.hasNewMessage is False: print "We never received robot joint positions... what gives." cleanup_and_exit(1) lastRobotJointPositions = robotSubscriber.lastMsg.position if np.linalg.norm( np.array(targetPosition) - np.array(lastRobotJointPositions[0:7])) > 0.1: print "Did not reach target position: ", targetPosition, " vs current pos ", lastRobotJointPositions cleanup_and_exit(1) # Test IK utilities, which go into pydrake import geometry_msgs import robot_msgs.srv poseStamped = geometry_msgs.msg.PoseStamped() poseStamped.pose.position.x = 0.5 poseStamped.pose.position.y = 0.0 poseStamped.pose.position.z = 1.5 poseStamped.pose.orientation.w = 1.0 poseStamped.pose.orientation.x = 0.0 poseStamped.pose.orientation.y = 0.0 poseStamped.pose.orientation.z = 0.0 srvRequest = robot_msgs.srv.RunIKRequest() srvRequest.pose_stamped = poseStamped success = robotService.moveToCartesianPosition(srvRequest, timeout=5) if not success: print "robotService moveToCartesianPosition returned failure ", success cleanup_and_exit(1) # Close the gripper from spartan.manipulation.schunk_driver import SchunkDriver schunkDriver = SchunkDriver() rospy.sleep(1.0) if schunkDriver.lastStatusMsg is None: print "Got no starting gripper position. Is sim running?" cleanup_and_exit(1) startingGripperPosition = schunkDriver.lastStatusMsg.position_mm schunkDriver.sendCloseGripperCommand() rospy.sleep(1.0) if schunkDriver.lastStatusMsg.position_mm >= 10.0: print "After trying to close gripper, gripper position was %f (started at %f)." % ( schunkDriver.lastStatusMsg.position_mm, startingGripperPosition) print "sendCloseGripperCommand appears ineffectual" cleanup_and_exit(1)