def __jointtraj_server(self):
limb = "both"
rate = 100.0
mode = "position"
if mode == 'velocity':
dyn_cfg_srv = Server(VelocityJointTrajectoryActionServerConfig,
lambda config, level: config)
elif mode == 'position':
dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig,
lambda config, level: config)
else:
dyn_cfg_srv = Server(PositionFFJointTrajectoryActionServerConfig,
lambda config, level: config)
jtas = []
if limb == 'both':
jtas.append(JointTrajectoryActionServer('right', dyn_cfg_srv,
rate, mode))
jtas.append(JointTrajectoryActionServer('left', dyn_cfg_srv,
rate, mode))
else:
jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
def cleanup():
for j in jtas:
j.clean_shutdown()
rospy.on_shutdown(cleanup)
print("Running. Ctrl-c to quit")
rospy.spin()
def start_server(limb, rate, mode):
print("Initializing node... ")
rospy.init_node("rsdk_%s_joint_trajectory_action_server%s" % (
mode,
"" if limb == 'both' else "_" + limb,
))
print("Initializing joint trajectory action server...")
if mode == 'velocity':
dyn_cfg_srv = Server(VelocityJointTrajectoryActionServerConfig,
lambda config, level: config)
else:
dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig,
lambda config, level: config)
jtas = []
if limb == 'both':
jtas.append(
JointTrajectoryActionServer('right', dyn_cfg_srv, rate, mode))
jtas.append(
JointTrajectoryActionServer('left', dyn_cfg_srv, rate, mode))
else:
jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
def cleanup():
for j in jtas:
j.clean_shutdown()
rospy.on_shutdown(cleanup)
print("Running. Ctrl-c to quit")
rospy.spin()
def start_server(limb='left', rate=100.0, mode='position', interpolation='bezier'):
if mode == 'velocity':
dyn_cfg_srv = Server(VelocityJointTrajectoryActionServerConfig,
lambda config, level: config)
elif mode == 'position':
dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig,
lambda config, level: config)
else:
dyn_cfg_srv = Server(PositionFFJointTrajectoryActionServerConfig,
lambda config, level: config)
jtas = []
if limb == 'both':
jtas.append(JointTrajectoryActionServer('right', dyn_cfg_srv,
rate, mode, interpolation))
jtas.append(JointTrajectoryActionServer('left', dyn_cfg_srv,
rate, mode, interpolation))
else:
jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode, interpolation))
def cleanup():
for j in jtas:
j.clean_shutdown()
rospy.on_shutdown(cleanup)
print("Running. Ctrl-c to quit")
rospy.spin()
def __init__(self, executor, shared_data, limb, rate, mode):
"""
Initalization of the two arms and grippers
limb (string): joint trajectory action limb. You can enter both,left or right (default="both")
rate (float): trajectory control rate in Hz. (default=100.0)
mode (string): Control mode for trajectory execution. You can enter position_w_id,position or velocity (default="position_w_id")
"""
self.baxterInitHandler = shared_data['BAXTER_INIT_HANDLER']
self.dyn_cfg_srv = None # for traj action server
self.jtas = [] # for track which limbs are used
# initialize dictionaries
self.traj = {} # for loading traj file
self.play_traj_thread = {} # execute play_traj asynchornously
self.stop_traj_thread = {} # execute stop_traj asynchornously
self.play_traj_status_act = {} # track play_traj actuator status.
self.baxterInitHandler.play_traj_status_sen = {
} # track play_traj sensor status.
self.trajLock = {} # for locking traj access
rospy.on_shutdown(self._cleanup)
ltlmop_logger.info("Initializing joint trajectory action server...")
if mode == 'velocity':
self.dyn_cfg_srv = Server(
VelocityJointTrajectoryActionServerConfig,
lambda config, level: config)
elif mode == 'position':
self.dyn_cfg_srv = Server(
PositionJointTrajectoryActionServerConfig,
lambda config, level: config)
else:
self.dyn_cfg_srv = Server(
PositionFFJointTrajectoryActionServerConfig,
lambda config, level: config)
if limb == 'both':
self.jtas.append(
JointTrajectoryActionServer('right', self.dyn_cfg_srv, rate,
mode))
self.jtas.append(
JointTrajectoryActionServer('left', self.dyn_cfg_srv, rate,
mode))
else:
self.jtas.append(
JointTrajectoryActionServer(limb, self.dyn_cfg_srv, rate,
mode))
self.pause = False
def start_server(limb, rate):
print("Initializing node... ")
rospy.init_node("rsdk_joint_trajectory_action_server%s" %
("" if limb == 'both' else "_" + limb, ))
print("Initializing joint trajectory action server...")
dynamic_cfg_srv = Server(JointTrajectoryActionServerConfig,
lambda config, level: config)
if limb == 'both':
JointTrajectoryActionServer('right', dynamic_cfg_srv, rate)
JointTrajectoryActionServer('left', dynamic_cfg_srv, rate)
else:
JointTrajectoryActionServer(limb, dynamic_cfg_srv, rate)
print("Running. Ctrl-c to quit")
rospy.spin()
def __init__(self, limb, rate, mode):
"""
Initalization of the two arms and grippers
limb (string): joint trajectory action limb. You can enter both,left or right (default="both")
rate (float): trajectory control rate in Hz. (default=100.0)
mode (string): Control mode for trajectory execution. You can enter position_w_id,position or velocity (default="position_w_id")
"""
self.dyn_cfg_srv = None # for traj action server
self.jtas = [] # for track which limbs are used
# initialize dictionaries
rospy.on_shutdown(self._cleanup)
mAndm_logger.info("Initializing joint trajectory action server...")
if mode == 'velocity':
self.dyn_cfg_srv = Server(
VelocityJointTrajectoryActionServerConfig,
lambda config, level: config)
elif mode == 'position':
self.dyn_cfg_srv = Server(
PositionJointTrajectoryActionServerConfig,
lambda config, level: config)
else:
self.dyn_cfg_srv = Server(
PositionFFJointTrajectoryActionServerConfig,
lambda config, level: config)
if limb == 'both':
self.jtas.append(
JointTrajectoryActionServer('right', self.dyn_cfg_srv, rate,
mode))
self.jtas.append(
JointTrajectoryActionServer('left', self.dyn_cfg_srv, rate,
mode))
else:
self.jtas.append(
JointTrajectoryActionServer(limb, self.dyn_cfg_srv, rate,
mode))
def start_server(limb, rate, mode, valid_limbs):
rospy.loginfo("Initializing node... ")
rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format(
mode,
"" if limb == 'all_limbs' else "_" + limb,
))
rospy.loginfo("Initializing joint trajectory action server...")
robot_name = intera_interface.RobotParams().get_robot_name().lower(
).capitalize()
config_module = "intera_interface.cfg"
if mode == 'velocity':
config_name = ''.join(
[robot_name, "VelocityJointTrajectoryActionServerConfig"])
elif mode == 'position':
config_name = ''.join(
[robot_name, "PositionJointTrajectoryActionServerConfig"])
else:
config_name = ''.join(
[robot_name, "PositionFFJointTrajectoryActionServerConfig"])
cfg = importlib.import_module('.'.join([config_module, config_name]))
dyn_cfg_srv = Server(cfg, lambda config, level: config)
jtas = []
if limb == 'all_limbs':
for current_limb in valid_limbs:
jtas.append(
JointTrajectoryActionServer(current_limb, dyn_cfg_srv, rate,
mode))
else:
jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
def cleanup():
for j in jtas:
j.clean_shutdown()
rospy.on_shutdown(cleanup)
rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit")
rospy.spin()
