def post_physics_update(self, simulator, deltaT): """Publishes the current joint state. :type simulator: iai_bullet_sim.basic_simulator.BasicSimulator :type deltaT: float """ if self.__enabled is False: return new_js = self.body.joint_state() safe_js = {} msg = JointStateMsg() msg.header.stamp = rospy.Time.now() for name, state in new_js.items(): joint = self.body.joints[name] if (state.position < joint.lowerLimit or state.position > joint.upperLimit) and joint.upperLimit >= joint.lowerLimit: state.position = min(max(joint.lowerLimit, state.position), joint.upperLimit) safe_js[name] = state.position msg.name.append(name) msg.position.append(state.position) msg.velocity.append(state.velocity) msg.effort.append(state.effort) self.publisher.publish(msg) if len(safe_js) > 0: #print('Out-of bounds joints:\n {}'.format('\n '.join(['{}: {} {}'.format(n, self.body.joints[n].lowerLimit, self.body.joints[n].upperLimit) for n in sorted(safe_js.keys())]))) self.body.set_joint_positions(safe_js)
def cb_js(self, msg): # Map von Jointnamen auf Variablen robot_joint_symbols = self.robot.joint_to_symbol_map # ueber alle Felder iterieren for x in range(len(msg.name)): # Wenn Joint tatsaechlich im Roboter definiert ist if msg.name[x] in robot_joint_symbols: # Variablenwert aktualisieren mit neuer Position self.cur_subs[robot_joint_symbols[ msg.name[x]]] = msg.position[x] # Kommandos generieren lassen. Variablenbelegung muss als {str: float} erfolgen. cmd = self.controller.get_cmd( {str(s): p for s, p in self.cur_subs.items()}) # Kommandonachricht anlegen cmd_msg = JointStateMsg() # Kommandozeitstempel setzen cmd_msg.header.stamp = rospy.Time.now() # Positionen und Kraefte auf 0 setzen cmd_msg.position = [0] * len(cmd) cmd_msg.effort = [0] * len(cmd) # Kommando in Nachricht schreiben for j, v in cmd.items(): cmd_msg.name.append(j) cmd_msg.velocity.append(v) # Kommando abschicken self.pub_cmd.publish(cmd_msg)
def cb_robot_joint_state(self, state_msg): if self._t_last_update is None: self._t_last_update = Time.now() return now = Time.now() dt = (now - self._t_last_update).to_sec() self._t_last_update = now self.state[self._poi_pos] = cos(now.to_sec()) if self.robot_js_aliases is None: return for name, p in zip(state_msg.name, state_msg.position): if name in self.robot_js_aliases: self.state[self.robot_js_aliases[name]] = p cmd = {} if self._current_target is None: if self.idle_controller is not None: print('Idling around...') cmd = self.idle_controller.get_cmd(self.state, deltaT=dt) else: if self.pushing_controller is not None: if not self._needs_odom or self._has_odom: try: now = Time.now() cmd = self.pushing_controller.get_cmd(self.state, deltaT=dt) time_taken = Time.now() - now print('Command generated. Time taken: {} Rate: {} hz'. format(time_taken.to_sec(), 1.0 / time_taken.to_sec())) except Exception as e: time_taken = Time.now() - now print( 'Command generation failed. Time taken: {} Rate: {} hz\nError: {}' .format(time_taken.to_sec(), 1.0 / time_taken.to_sec(), e)) #print(self.pushing_controller.last_matrix_str()) else: print('Waiting for odom...') if len(cmd) > 0: #print('commands:\n {}'.format('\n '.join(['{}: {}'.format(s, v) for s, v in cmd.items()]))) cmd_msg = JointStateMsg() cmd_msg.header.stamp = Time.now() cmd_msg.name, cmd_msg.velocity = zip( *[(self.inverse_js_aliases[erase_type(s)], v) for s, v in cmd.items()]) cmd_msg.position = [0] * len(cmd_msg.name) cmd_msg.effort = cmd_msg.position self.pub_cmd.publish(cmd_msg)
def _tmserver_callback(self, items): # Build joint state from TM server callback joint_state = JointStateMsg() joint_state.header.seq = self._seq_id self._seq_id += 1 joint_state.header.stamp = rospy.Time.now() joint_state.name = self.JOINTS joint_state.position = [np.radians(x) for x in items['Joint_Angle']] joint_state.velocity = [np.radians(x) for x in items['Joint_Speed']] joint_state.effort = items['Joint_Torque'] super()._on_joint_state(joint_state) self._publish_tm_pose('real_flange', items['Coord_Robot_Flange'])
def post_physics_update(self, simulator, deltaT): """Computes the delta between the given commands and the current joint velocities and publishes the result. :type simulator: iai_bullet_sim.basic_simulator.BasicSimulator :type deltaT: float """ if self._enabled: deltaMsg = JointStateMsg() deltaMsg.header.stamp = rospy.Time.now() js = self.body.joint_state() deltaMsg.position = [0]*len(self.next_cmd) deltaMsg.effort = deltaMsg.position for j, dv in self.next_cmd.items(): deltaMsg.name.append(j) deltaMsg.velocity.append(dv - js[j].velocity) self.delta_publisher.publish(deltaMsg)
def __init__(self, robot_prefix, joint_topic, joint_vel_symbols, base_topic=None, base_symbols=None, base_watchdog=rospy.Duration(0.4), reference_frame='odom'): self.control_alias = { s: str(Path(gm.erase_type(s))[-1]) for s in joint_vel_symbols } self._robot_cmd_msg = JointStateMsg() self._robot_cmd_msg.name = list(self.control_alias.keys()) self._state_cb = None self.robot_prefix = robot_prefix if base_symbols is not None: self.base_aliases = base_symbols self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) self.reference_frame = reference_frame self._base_cmd_msg = TwistMsg() self.pub_base_command = rospy.Publisher(base_topic, TwistMsg, queue_size=1, tcp_nodelay=True) self._base_last_cmd_stamp = None self._base_cmd_lock = RLock() self._base_thread = threading.Thread( target=self._base_cmd_repeater, args=(base_watchdog, )) self._base_thread.start() else: self.base_aliases = None self.pub_joint_command = rospy.Publisher(joint_topic, JointStateMsg, queue_size=1, tcp_nodelay=True) self.sub_robot_js = rospy.Subscriber('/joint_states', JointStateMsg, callback=self.cb_robot_js, queue_size=1)
def __init__(self, km, robot_path, eefs): self.km = km self.pub_cmd = rospy.Publisher('commands', JointStateMsg, queue_size=1, tcp_nodelay=True) self.robot_name = str(robot_path) self.eefs = {e.link: e for e in eefs} self.global_command = False self.current_eef = next(iter(self.eefs.values())) self.initialized = False self.services = [rospy.Service('get_endeffectors', GetEndeffectors, self.srv_get_endeffectors)] soft_constraints = {} for e in self.eefs.values(): soft_constraints.update(e.generate_constraints()) self.controller = BCControllerWrapper(km, robot_path, soft_constraints) self.sub_dm = rospy.Subscriber('/remote_control', RemoteControlMsg, self.cb_device_motion, queue_size=1) self.sub_js = rospy.Subscriber('joint_states', JointStateMsg, self.cb_joint_state, queue_size=1) self.cmd_msg = JointStateMsg()
urdf = load_urdf_str(rosparam_description) km = GeometryModel() load_urdf(km, Path('iiwa'), urdf) km.clean_structure() km.dispatch_events() sim = KineverseKinematicSim(km, Path('iiwa'), state_topic='/iiwa/joint_states') sorted_names = sorted(sim.state_info.keys()) js_msg = JointStateMsg() js_msg.name = sorted_names def cb_pos_array_command(msg): js_msg.header.stamp = rospy.Time.now() js_msg.position = msg.data print(f'JS names {js_msg.name}') print(f'JS positions {js_msg.position}') sim.process_command(js_msg) init_msg = Float64MultiArrayMsg() init_msg.data = [ np.deg2rad(x) for x in [-18, 25.04, 3, -98.2, 0.38, 60.2, 35.85] ] cb_pos_array_command(init_msg)
km = GeometryModel() kitchen_prefix = Path(kitchen_urdf_model.name) load_urdf(km, kitchen_prefix, kitchen_urdf_model) plot_graph(generate_dependency_graph(km, {'connect': 'blue'}), '{}/kitchen_sandbox_dep_graph.pdf'.format(plot_dir)) # ROBOT STATE PUBLISHER sp_p = subprocess.Popen([ pub_path, '__name:={}_state_publisher'.format(kitchen_urdf_model.name), 'robot_description:=/{}/robot_description'.format( kitchen_urdf_model.name), '_tf_prefix:={}'.format(kitchen_urdf_model.name), 'joint_states:=/{}/joint_states'.format(kitchen_urdf_model.name) ]) jsmsg = JointStateMsg() jsmsg.name = kitchen_urdf_model.joint_map.keys() jsmsg.position = [0.0] * len(jsmsg.name) int_factor = 0.02 while not rospy.is_shutdown(): now = rospy.Time.now() if (now - jsmsg.header.stamp).to_sec() >= int_factor: jsmsg.header.stamp = now jsmsg.position = [trajectory[j][x] for j in jsmsg.name] kitchen_js_pub.publish(jsmsg) sp_p.terminate() sp_p.wait()