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()