def send_to_neutral(): """ DON'T USE THIS ON REAL ROBOT!!! """ temp_pub = rospy.Publisher('/panda_simulator/motion_controller/arm/joint_commands',JointCommand, queue_size = 1, tcp_nodelay = True) # Create JointCommand message to publish commands pubmsg = JointCommand() pubmsg.names = names # names of joints (has to be 7) pubmsg.position = neutral_pose # JointCommand msg has other fields (velocities, efforts) for # when controlling in other control mode pubmsg.mode = pubmsg.POSITION_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE) curr_val = deepcopy(vals) while all(abs(neutral_pose[i]-curr_val[i]) > 0.01 for i in range(len(curr_val))): temp_pub.publish(pubmsg) curr_val = deepcopy(vals)
def __init__(self): # Subscribe to robot joint state self._sub_js = rospy.Subscriber( '/panda_simulator/custom_franka_state_controller/joint_states', JointState, self._joint_states_cb) # Subscribe to robot state (Refer JointState.msg to find all available data. # Note: All msg fields are not populated when using the simulated environment) self._sub_rs = rospy.Subscriber( '/panda_simulator/custom_franka_state_controller/robot_state', RobotState, self._robot_states_cb) # Published joint command self._pub = rospy.Publisher( '/panda_simulator/motion_controller/arm/joint_commands', JointCommand, queue_size=1, tcp_nodelay=True) # Robot state self.arm_joint_names = [] self.position = [0, 0, 0, 0, 0, 0, 0] self.velocity = [0, 0, 0, 0, 0, 0, 0] self.arm_dof = len(arm_joint_names) # Create JointCommand message to publish commands self.pubmsg = JointCommand() self.pubmsg.names = arm_joint_names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts)) self.pubmsg.mode = self.pubmsg.VELOCITY_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE # Internal Variables self.go_pos = [1, 1, 1] self.pos_limit = [0, 0, 0] self.neg_limit = [0, 0, 0] self.desired_velocity = [0, 0, 0, 0, 0, 0, 0] self.filt = 0.49999 self.time = 0
def __init__(self, synchronous_pub=False): self._params = RobotParams() self._ns = self._params.get_base_namespace() self._joint_limits = self._params.get_joint_limits() joint_names = self._joint_limits.joint_names if not joint_names: rospy.logerr("{}: Cannot detect joint names for arm on this " "robot. Exiting Arm.init().".format( self.__class__.__name__)) return self._joint_names = joint_names self.name = self._params.get_robot_name() self._joint_angle = dict() self._joint_velocity = dict() self._joint_effort = dict() self._cartesian_pose = dict() self._cartesian_velocity = dict() self._cartesian_effort = dict() self._stiffness_frame_effort = dict() self._errors = dict() self._collision_state = False self._tip_states = None self._jacobian = None self._cartesian_contact = None self._robot_mode = False self._command_msg = JointCommand() # neutral pose joint positions self._neutral_pose_joints = self._params.get_neutral_pose() self._frames_interface = FrankaFramesInterface() try: self._collision_behaviour_interface = CollisionBehaviourInterface() except rospy.ROSException: rospy.loginfo( "{}: Collision Service Not found. It will not be possible to change collision behaviour of robot!" .format(self.__class__.__name__)) self._collision_behaviour_interface = None self._ctrl_manager = FrankaControllerManagerInterface( ns=self._ns, sim=self._params._in_sim) self._speed_ratio = 0.15 self._F_T_NE = np.eye(1).flatten().tolist() self._NE_T_EE = np.eye(1).flatten().tolist() queue_size = None if synchronous_pub else 1 with warnings.catch_warnings(): warnings.simplefilter("ignore") self._joint_command_publisher = rospy.Publisher( self._ns + '/motion_controller/arm/joint_commands', JointCommand, tcp_nodelay=True, queue_size=queue_size) self._pub_joint_cmd_timeout = rospy.Publisher( self._ns + '/motion_controller/arm/joint_command_timeout', Float64, latch=True, queue_size=10) self._robot_state_subscriber = rospy.Subscriber( self._ns + '/custom_franka_state_controller/robot_state', RobotState, self._on_robot_state, queue_size=1, tcp_nodelay=True) joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states' self._joint_state_sub = rospy.Subscriber(joint_state_topic, JointState, self._on_joint_states, queue_size=1, tcp_nodelay=True) self._cartesian_state_sub = rospy.Subscriber( self._ns + '/custom_franka_state_controller/tip_state', EndPointState, self._on_endpoint_state, queue_size=1, tcp_nodelay=True) rospy.on_shutdown(self._clean_shutdown) err_msg = ("%s arm init failed to get current joint_states " "from %s") % (self.name.capitalize(), joint_state_topic) franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s arm, init failed to get current tip_state " "from %s") % (self.name.capitalize(), self._ns + 'tip_state') franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s arm, init failed to get current robot_state " "from %s") % (self.name.capitalize(), self._ns + 'robot_state') franka_dataflow.wait_for(lambda: self._jacobian is not None, timeout_msg=err_msg, timeout=5.0) # start moveit server with panda_link8 (flange) as the end-effector, unless it is in simulation. However, using move_to_cartesian_pose() method compensates for this and moves the robot's defined end-effector frame (EE frame; see set_EE_frame() and set_EE_at_frame()) to the target pose. try: self._movegroup_interface = PandaMoveGroupInterface( use_panda_hand_link=True if self._params._in_sim else False) except: rospy.loginfo( "{}: MoveGroup was not found! This is okay if moveit service is not required!" .format(self.__class__.__name__)) self._movegroup_interface = None self.set_joint_position_speed(self._speed_ratio)
def __init__(self, DS_type = 1, A = [0.0] * 7, DS_attractor = [0.0] * 7, ctrl_rate = 1000, cmd_type = 1, epsilon = 0.005, vel_limits = panda_maxVel, null_space = [0.0] * 7): # Subscribe to robot joint state self._sub_js = rospy.Subscriber('/panda_simulator/custom_franka_state_controller/joint_states', JointState, self._joint_states_cb, queue_size=1,tcp_nodelay=True) # if not using franka_ros_interface, you have to subscribe to the right topics # to obtain the current end-effector state and robot jacobian for computing # commands self._sub_cart_state = rospy.Subscriber('panda_simulator/custom_franka_state_controller/tip_state', EndPointState, self._on_endpoint_state, queue_size=1,tcp_nodelay=True) self._sub_robot_state = rospy.Subscriber('panda_simulator/custom_franka_state_controller/robot_state',RobotState, self._on_robot_state, queue_size=1,tcp_nodelay=True) # Published joint command self._pub = rospy.Publisher('/panda_simulator/motion_controller/arm/joint_commands',JointCommand, queue_size = 1, tcp_nodelay = True) self.DS_type = DS_type if self.DS_type == 2: self._pub_target = rospy.Publisher('DS_target', PointStamped) # Robot state self.arm_joint_names = [] self.position = [0,0,0,0,0,0,0] self.velocity = [0,0,0,0,0,0,0] self.arm_dof = len(arm_joint_names) # Create JointCommand message to publish commands self.cmd_type = cmd_type self.pubmsg = JointCommand() self.pubmsg.names = arm_joint_names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts)) if self.cmd_type == 1: self.pubmsg.mode = self.pubmsg.VELOCITY_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE elif self.cmd_type == 2: self.pubmsg.mode = self.pubmsg.POSITION_MODE # Robot Jacobian and EE representations self.jacobian = [] self.ee_pose = [] self.ee_pos = [] self.ee_rot = [] self.ee_quat = [] self.ee_lin_vel = [] self.ee_ang_vel = [] # Control Variables self.desired_velocity = [0,0,0,0,0,0,0] self.desired_position = [0,0,0,0,0,0,0] self.vel_limits = np.array(panda_maxVel) self.add_nullspace = 0 # Variables for DS-Motion Generator self.dt = 1.0/float(ctrl_rate) self.DS_attractor = np.array(DS_attractor) self.rate = rospy.Rate(ctrl_rate) self.A = np.array(A) self.epsilon = epsilon self.pos_error = 0 self.quat_error = 0 self._stop = 0 self.force_limits = 0 # Spin once to update robot state first_msg = rospy.wait_for_message('/joint_states', JointState) self._populate_joint_state(first_msg) # Given size of DS-attractor define FK and Jacobian functions # Only for JT-DS type if self.DS_type == 2: self.DS_pos_att = self.DS_attractor[0:3] if len(self.DS_attractor) == 3: self.attr_type = 'pos' if len(self.DS_attractor) == 7: self.attr_type = 'full' self.DS_quat_att = self.DS_attractor[3:7] self.add_nullspace = 0 self.null_space = np.array(null_space) self.S_limits = np.identity(self.arm_dof) # For debugging purposes: Visualize init parameters for trajectory rospy.loginfo('DS type (1:joint-space, 2: joint-space task oriented): {}'.format(self.DS_type)) # Specific DS parameters if self.DS_type == 1: rospy.loginfo('DS attractor: {}'.format(self.DS_attractor)) if self.DS_type == 2: rospy.loginfo('DS position attractor: {}'.format(self.DS_pos_att)) rospy.loginfo('Null-Space target: {}'.format(self.null_space)) if len(self.DS_attractor) == 7: rospy.loginfo('DS quaternion attractor: {}'.format(self.DS_quat_att)) # Generic DS parameters rospy.loginfo('DS system matrix A:\n {}'.format(self.A)) rospy.loginfo('Stopping epsilon: {}'.format(self.epsilon)) # For debugging purposes: Visualize parameters for Motion Controllers rospy.loginfo('Control command type (1: velocities, 2: positions): {}'.format(self.cmd_type))
rospy.loginfo("Sending robot to neutral pose") send_to_neutral() # DON'T DO THIS ON REAL ROBOT!! (use move_to_neutral() method from ArmInterface of franka_ros_interface package) rospy.sleep(2.0) initial_pose = vals rospy.loginfo("Commanding...\n") elapsed_time_ = rospy.Duration(0.0) period = rospy.Duration(0.005) count = 0 # Create JointCommand message to publish commands pubmsg = JointCommand() pubmsg.names = names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts)) while not rospy.is_shutdown(): elapsed_time_ += period delta = 3.14 / 16.0 * (1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2 for j in range(len(vals)): if j == 4: vals[j] = initial_pose[j] - delta else: vals[j] = initial_pose[j] + delta pubmsg.position = vals # JointCommand msg has other fields (velocities, efforts) for # when controlling in other control mode
robot_state_sub = rospy.Subscriber( 'panda_simulator/custom_franka_state_controller/robot_state', RobotState, _on_robot_state, queue_size=1, tcp_nodelay=True) desired_state_sub = rospy.Subscriber('panda_simulator/desired_twist', Twist, _on_desired_twist, queue_size=1, tcp_nodelay=True) # create joint command message and fix its type to joint torque mode command_msg = JointCommand() command_msg.names = ['panda_joint1','panda_joint2','panda_joint3',\ 'panda_joint4','panda_joint5','panda_joint6','panda_joint7'] command_msg.mode = JointCommand.TORQUE_MODE # Also create a publisher to publish joint commands joint_command_publisher = rospy.Publisher( 'panda_simulator/motion_controller/arm/joint_commands', JointCommand, tcp_nodelay=True, queue_size=1) # wait for messages to be populated before proceeding rospy.loginfo("Subscribing to robot state topics...") while (True): if not (JACOBIAN is None or CARTESIAN_POSE is None):
def __init__(self, DS_type=1, A_p=[0.0] * 3, A_o=[0.0] * 3, DS_attractor=[0.0] * 7, ctrl_rate=1000, epsilon=0.005, ctrl_orient=1, learned_gamma=[], pub_pose=1): # Subscribe to robot joint state self._sub_js = rospy.Subscriber( '/panda_simulator/custom_franka_state_controller/joint_states', JointState, self._joint_states_cb, queue_size=1, tcp_nodelay=True) # if not using franka_ros_interface, you have to subscribe to the right topics # to obtain the current end-effector state and robot jacobian for computing # commands self._sub_cart_state = rospy.Subscriber( 'panda_simulator/custom_franka_state_controller/tip_state', EndPointState, self._on_endpoint_state, queue_size=1, tcp_nodelay=True) self._sub_robot_state = rospy.Subscriber( 'panda_simulator/custom_franka_state_controller/robot_state', RobotState, self._on_robot_state, queue_size=1, tcp_nodelay=True) # ---Publishes twist command to filter node self._pub_twist = rospy.Publisher('/panda_simulator/desired_twist', Twist, queue_size=10) self._pub_target = rospy.Publisher('DS_target', PointStamped, queue_size=1) self._pub_ee_pos = rospy.Publisher('curr_ee_pos', PointStamped, queue_size=1) self._pub_wrench = rospy.Publisher('/panda_simulator/desired_wrench', WrenchStamped, queue_size=10) # Robot state self.arm_joint_names = [] self.position = [0, 0, 0, 0, 0, 0, 0] self.velocity = [0, 0, 0, 0, 0, 0, 0] self.arm_dof = len(arm_joint_names) self.pub_pose = pub_pose # Create JointCommand message to publish commands self._pubmsg = JointCommand() self._pubmsg.names = arm_joint_names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts)) self._pubmsg.mode = self._pubmsg.TORQUE_MODE # Also create a publisher to publish joint commands self._joint_command_publisher = rospy.Publisher( 'panda_simulator/motion_controller/arm/joint_commands', JointCommand, tcp_nodelay=True, queue_size=1) self.control_orient = ctrl_orient # Robot Jacobian and EE representations self.jacobian = [] self.ee_pose = [] self.ee_pos = [] self.ee_rot = [] self.ee_quat = [] self.ee_lin_vel = [] self.ee_ang_vel = [] # Robot commands in task-space self.twist_msg = Twist() self.twist_msg.linear.x = 0 self.twist_msg.linear.y = 0 self.twist_msg.linear.z = 0 self.twist_msg.angular.x = 0 self.twist_msg.angular.y = 0 self.twist_msg.angular.z = 0 # Might remove this, doesn't seem to work like I expect it to.. self.wrench_msg = WrenchStamped() self.wrench_msg.wrench = Wrench() self.wrench_msg.wrench.force = Vector3() self.wrench_msg.wrench.torque = Vector3() # self.wrench_msg.header.frame_id = '/panda_link7' # self.wrench_msg.header.frame_id = '/world_on_ee' self.wrench_msg.header.stamp = rospy.Time.now() # Control Variables self.desired_position = [0, 0, 0, 0, 0, 0] self.desired_velocity = [0, 0, 0, 0, 0, 0] # self.max_lin_vel = 1.70 # self.max_ang_vel = 2.5 self.max_lin_vel = 1.0 self.max_ang_vel = 1.0 # Variables for DS-Motion Generator self.dt = 1.0 / float(ctrl_rate) self.DS_type = DS_type self.DS_attractor = np.array(DS_attractor) self.rate = rospy.Rate(ctrl_rate) self.epsilon = epsilon self._stop = 0 # Linear DS self.A_p = np.array(A_p) self.pos_error = 0 self.scale_DS = 1 self.learned_gamma = learned_gamma self.classifier = learned_gamma['classifier'] self.max_dist = learned_gamma['max_dist'] self.reference_points = learned_gamma['reference_points'] # Quaternion DS self.A_o = np.array(A_o) self.quat_error = 0 # Spin once to update robot state first_msg = rospy.wait_for_message( '/panda_simulator/custom_franka_state_controller/joint_states', JointState) self._populate_joint_state(first_msg) self.DS_pos_att = self.DS_attractor[0:3] if len(self.DS_attractor) == 3: self.attr_type = 'pos' if len(self.DS_attractor) == 7: self.attr_type = 'full' self.DS_quat_att = self.DS_attractor[3:7] # For debugging purposes: Visualize init parameters for trajectory rospy.loginfo('DS type (1:linear, 2: non-linear): {}'.format( self.DS_type)) # Specific DS parameters rospy.loginfo('DS position attractor: {}'.format(self.DS_pos_att)) rospy.loginfo('DS quaternion attractor: {}'.format(self.DS_quat_att)) # Generic DS parameters rospy.loginfo('DS system matrix A_p:\n {}'.format(self.A_p)) rospy.loginfo('DS system matrix A_o:\n {}'.format(self.A_o)) rospy.loginfo('Stopping epsilon: {}'.format(self.epsilon))
#! /usr/bin/env python import rospy from franka_core_msgs.msg import JointCommand if __name__ == "__main__": rospy.init_node("force_neutral_pose_startup") pub = rospy.Publisher( 'panda_simulator/motion_controller/arm/joint_commands', JointCommand, tcp_nodelay=True, queue_size=10) command_msg = JointCommand() command_msg.names = ["panda_joint%d" % (idx) for idx in range(1, 8)] command_msg.position = [0.000, -0.785, 0.0, -2.356, 0.0, 1.57, 0.785] command_msg.mode = JointCommand.POSITION_MODE rospy.sleep(0.5) start = rospy.Time.now().to_sec() rospy.loginfo("Attempting to force robot to neutral pose...") rospy.sleep(0.5) while not rospy.is_shutdown() and (rospy.Time.now().to_sec() - start < 1.): # print rospy.Time.now() command_msg.header.stamp = rospy.Time.now() pub.publish(command_msg)
def __init__(self, synchronous_pub=False): """ Constructor. @type synchronous_pub: bool @param synchronous_pub: designates the JointCommand Publisher as Synchronous if True and Asynchronous if False. Synchronous Publishing means that all joint_commands publishing to the robot's joints will block until the message has been serialized into a buffer and that buffer has been written to the transport of every current Subscriber. This yields predicable and consistent timing of messages being delivered from this Publisher. However, when using this mode, it is possible for a blocking Subscriber to prevent the joint_command functions from exiting. Unless you need exact JointCommand timing, default to Asynchronous Publishing (False). """ self._params = RobotParams() self._ns = self._params.get_base_namespace() self._joint_limits = self._params.get_joint_limits() joint_names = self._joint_limits.joint_names if not joint_names: rospy.logerr("Cannot detect joint names for arm on this " "robot. Exiting Arm.init().") return self._joint_names = joint_names self.name = self._params.get_robot_name() self._joint_angle = dict() self._joint_velocity = dict() self._joint_effort = dict() self._cartesian_pose = dict() self._cartesian_velocity = dict() self._cartesian_effort = dict() self._stiffness_frame_effort = dict() self._errors = dict() self._collision_state = False self._tip_states = None self._jacobian = None self._cartesian_contact = None self._robot_mode = False self._command_msg = JointCommand() # neutral pose joint positions self._neutral_pose_joints = self._params.get_neutral_pose() self._frames_interface = FrankaFramesInterface() self._ctrl_manager = FrankaControllerManagerInterface( ns=self._ns, sim=True if self._params._in_sim else False) self._speed_ratio = 0.15 queue_size = None if synchronous_pub else 1 with warnings.catch_warnings(): warnings.simplefilter("ignore") self._joint_command_publisher = rospy.Publisher( self._ns + '/motion_controller/arm/joint_commands', JointCommand, tcp_nodelay=True, queue_size=queue_size) self._pub_joint_cmd_timeout = rospy.Publisher( self._ns + '/motion_controller/arm/joint_command_timeout', Float64, latch=True, queue_size=10) self._robot_state_subscriber = rospy.Subscriber( self._ns + '/custom_franka_state_controller/robot_state', RobotState, self._on_robot_state, queue_size=1, tcp_nodelay=True) joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states' if not self._params._in_sim else self._ns + '/joint_states' self._joint_state_sub = rospy.Subscriber(joint_state_topic, JointState, self._on_joint_states, queue_size=1, tcp_nodelay=True) self._cartesian_state_sub = rospy.Subscriber( self._ns + '/custom_franka_state_controller/tip_state', EndPointState, self._on_endpoint_state, queue_size=1, tcp_nodelay=True) rospy.on_shutdown(self._clean_shutdown) err_msg = ("%s arm init failed to get current joint_states " "from %s") % (self.name.capitalize(), joint_state_topic) franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s arm, init failed to get current tip_state " "from %s") % (self.name.capitalize(), self._ns + 'tip_state') franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s arm, init failed to get current robot_state " "from %s") % (self.name.capitalize(), self._ns + 'robot_state') franka_dataflow.wait_for(lambda: self._jacobian is not None, timeout_msg=err_msg, timeout=5.0)
def __init__(self, synchronous_pub=False): """ """ self.hand = franka_interface.GripperInterface() self._params = RobotParams() self._ns = self._params.get_base_namespace() self._joint_limits = self._params.get_joint_limits() joint_names = self._joint_limits.joint_names if not joint_names: rospy.logerr("Cannot detect joint names for arm on this " "robot. Exiting Arm.init().") return self._joint_names = joint_names self.name = self._params.get_robot_name() self._joint_angle = dict() self._joint_velocity = dict() self._joint_effort = dict() self._cartesian_pose = dict() self._cartesian_velocity = dict() self._cartesian_effort = dict() self._stiffness_frame_effort = dict() self._errors = dict() self._collision_state = False self._tip_states = None self._jacobian = None self._cartesian_contact = None self._robot_mode = False self._command_msg = JointCommand() # neutral pose joint positions self._neutral_pose_joints = self._params.get_neutral_pose() self._frames_interface = FrankaFramesInterface() try: self._collision_behaviour_interface = CollisionBehaviourInterface() except rospy.ROSException: rospy.loginfo( "Collision Service Not found. It will not be possible to change collision behaviour of robot!" ) self._collision_behaviour_interface = None self._ctrl_manager = FrankaControllerManagerInterface( ns=self._ns, sim=self._params._in_sim) self._speed_ratio = 0.15 queue_size = None if synchronous_pub else 1 with warnings.catch_warnings(): warnings.simplefilter("ignore") self._joint_command_publisher = rospy.Publisher( self._ns + '/motion_controller/arm/joint_commands', JointCommand, tcp_nodelay=True, queue_size=queue_size) self._pub_joint_cmd_timeout = rospy.Publisher( self._ns + '/motion_controller/arm/joint_command_timeout', Float64, latch=True, queue_size=10) self._robot_state_subscriber = rospy.Subscriber( self._ns + '/custom_franka_state_controller/robot_state', RobotState, self._on_robot_state, queue_size=1, tcp_nodelay=True) joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states' self._joint_state_sub = rospy.Subscriber(joint_state_topic, JointState, self._on_joint_states, queue_size=1, tcp_nodelay=True) self._cartesian_state_sub = rospy.Subscriber( self._ns + '/custom_franka_state_controller/tip_state', EndPointState, self._on_endpoint_state, queue_size=1, tcp_nodelay=True) # Cartesian Impedance Controller Publishers self._cartesian_impedance_pose_publisher = rospy.Publisher( "equilibrium_pose", PoseStamped, queue_size=10) self._cartesian_stiffness_publisher = rospy.Publisher( "impedance_stiffness", CartImpedanceStiffness, queue_size=10) # Force Control Publisher self._force_controller_publisher = rospy.Publisher("wrench_target", Wrench, queue_size=10) # Torque Control Publisher self._torque_controller_publisher = rospy.Publisher("torque_target", TorqueCmd, queue_size=20) # Joint Impedance Controller Publishers self._joint_impedance_publisher = rospy.Publisher( "joint_impedance_position_velocity", JICmd, queue_size=20) self._joint_stiffness_publisher = rospy.Publisher( "joint_impedance_stiffness", JointImpedanceStiffness, queue_size=10) rospy.on_shutdown(self._clean_shutdown) err_msg = ("%s arm init failed to get current joint_states " "from %s") % (self.name.capitalize(), joint_state_topic) franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s arm, init failed to get current tip_state " "from %s") % (self.name.capitalize(), self._ns + 'tip_state') franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0, timeout_msg=err_msg, timeout=5.0) err_msg = ("%s arm, init failed to get current robot_state " "from %s") % (self.name.capitalize(), self._ns + 'robot_state') franka_dataflow.wait_for(lambda: self._jacobian is not None, timeout_msg=err_msg, timeout=5.0) self.set_joint_position_speed(self._speed_ratio)