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:
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
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