def update_rviz_markers(self, req):
l = req.object_names
res = UpdateRvizMarkersResponse()
res.error_codes = UpdateRvizMarkersResponse.SUCCESS
ma = MarkerArray()
if len(l) == 0:
l = self.get_world().get_object_names()
for name in l:
try:
m = self.get_world().get_object(name).as_marker_msg()
m.action = m.ADD
m.header.frame_id = 'map'
m.ns = u'world' + m.ns
ma.markers.append(m)
except TypeError as e:
logging.logerr(
'failed to convert object {} to marker: {}'.format(
name, str(e)))
res.error_codes = UpdateRvizMarkersResponse.MARKER_CONVERSION_ERROR
except KeyError as e:
logging.logerr(
'could not update rviz marker for object {}: no object with that name was found'
.format(name))
res.error_codes = UpdateRvizMarkersResponse.NAME_NOT_FOUND_ERROR
self.pub_collision_marker.publish(ma)
return res
def are_joint_limits_violated(self, p_lb, p_ub):
violations = (p_ub - p_lb)[p_lb.data > p_ub.data]
if len(violations) > 0:
logging.logerr(u'The following joints are outside of their limits: \n {}'.format(violations))
return True
logging.loginfo(u'All joints are within limits')
return False
def is_nan_in_array(self, name, p_array):
p_filtered = p_array.apply(lambda x: zip(x.index[x.isnull()].tolist(), x[x.isnull()]), 1)
p_filtered = p_filtered[p_filtered.apply(lambda x: len(x)) > 0]
if len(p_filtered) > 0:
logging.logerr(u'{} has the following nans:'.format(name))
self.print_pandas_array(p_filtered)
return True
logging.loginfo(u'{} has no nans'.format(name))
return False
def load_compiled_function(file_name):
if os.path.isfile(file_name):
try:
with open(file_name, u'r') as file:
fast_f = pickle.load(file)
return fast_f
except EOFError as e:
os.remove(file_name)
logging.logerr(u'{} deleted because it was corrupted'.format(file_name))
def convert_dae_to_obj(path):
path = path.replace(u'\'', u'')
file_name = path.split(u'/')[-1]
name, file_format = file_name.split(u'.')
if u'dae' in file_format:
input_path = resolve_ros_iris(path)
new_path = u'/tmp/giskardpy/{}.obj'.format(name)
create_path(new_path)
try:
subprocess.check_call(
[u'meshlabserver', u'-i', input_path, u'-o', new_path])
except Exception as e:
logging.logerr(u'meshlab not installed, can\'t convert dae to obj')
return new_path
return path
def get_object_info(self, req):
res = GetObjectInfoResponse()
res.error_codes = GetObjectInfoResponse.SUCCESS
try:
object = self.get_world().get_object(req.object_name)
res.joint_state_topic = ''
if req.object_name in self.object_js_subs.keys():
res.joint_state_topic = self.object_js_subs[req.object_name].name
res.pose.pose = object.base_pose
res.pose.header.frame_id = self.get_god_map().get_data(identifier.map_frame)
for key, value in object.joint_state.items():
res.joint_state.name.append(key)
res.joint_state.position.append(value.position)
res.joint_state.velocity.append(value.velocity)
res.joint_state.effort.append(value.effort)
except KeyError as e:
logging.logerr('no object with the name {} was found'.format(req.object_name))
res.error_codes = GetObjectInfoResponse.NAME_NOT_FOUND_ERROR
return res
def __init__(self):
self.action_clients = []
self.joint_names = []
while not rospy.has_param('~state_topics'):
logging.loginfo('waiting for param ' + '~state_topics')
rospy.sleep(1)
while not rospy.has_param('~client_topics'):
logging.loginfo('waiting for param ' + '~client_topics')
rospy.sleep(1)
self.state_topics = rospy.get_param('~state_topics', [])
self.client_topics = rospy.get_param('~client_topics', [])
self.number_of_clients = len(self.state_topics)
if self.number_of_clients != len(self.client_topics):
logging.logerr('number of state and action topics do not match')
exit()
if self.number_of_clients == 0:
logging.logerr(
'the state_topic or client_topic parameter is empty')
exit()
self.client_type = []
for i in range(self.number_of_clients):
waiting_for_topic = True
while waiting_for_topic:
try:
rospy.wait_for_message(self.state_topics[i],
AnyMsg,
timeout=10)
waiting_for_topic = False
type = rostopic.get_info_text(self.client_topics[i] +
'/goal').split('\n')[0][6:]
self.client_type.append(type)
except rostopic.ROSTopicException as e:
logging.logerr(
'Exception: {} .Maybe unknown topic \'{}/goal\' \nmissing / in front of topic name?'
.format(e, self.client_topics[i]))
exit()
except rospy.ROSException as e:
if e.message == 'rospy shutdown':
exit()
logging.loginfo('waiting for state topic {}'.format(
self.state_topics[i]))
self.state_type = []
for i in range(self.number_of_clients):
try:
type = rostopic.get_info_text(
self.state_topics[i]).split('\n')[0][6:]
self.state_type.append(type)
except rostopic.ROSTopicException:
logging.logerr(
'unknown topic \'{}/goal\' \nmissing / in front of topic name?'
.format(self.state_topics[i]))
exit()
for i in range(self.number_of_clients):
if self.client_type[
i] == 'control_msgs/FollowJointTrajectoryActionGoal':
self.action_clients.append(
actionlib.SimpleActionClient(
self.client_topics[i],
control_msgs.msg.FollowJointTrajectoryAction))
elif self.client_type[
i] == 'pr2_controllers_msgs/JointTrajectoryActionGoal':
self.action_clients.append(
actionlib.SimpleActionClient(
self.client_topics[i],
pr2_controllers_msgs.msg.JointTrajectoryAction))
else:
logging.logerr(
'wrong client topic type:' + self.client_type[i] +
'\nmust be either control_msgs/FollowJointTrajectoryActionGoal or pr2_controllers_msgs/JointTrajectoryActionGoal'
)
exit()
self.joint_names.append(
rospy.wait_for_message(
self.state_topics[i], control_msgs.msg.
JointTrajectoryControllerState).joint_names)
logging.loginfo('connected to {}'.format(self.client_topics[i]))
self.current_controller_state = control_msgs.msg.JointTrajectoryControllerState(
)
total_number_joints = 0
for joint_name_list in self.joint_names:
total_number_joints += len(joint_name_list)
self.current_controller_state.joint_names.extend(joint_name_list)
self.current_controller_state.desired.positions = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.accelerations = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.effort = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.velocities = [
0 for i in range(total_number_joints)
]
self.current_controller_state.actual = copy.deepcopy(
self.current_controller_state.desired)
self.current_controller_state.error = copy.deepcopy(
self.current_controller_state.desired)
self.state_pub = rospy.Publisher(
'/whole_body_controller/state',
control_msgs.msg.JointTrajectoryControllerState,
queue_size=10)
for i in range(self.number_of_clients):
if self.state_type[
i] == 'control_msgs/JointTrajectoryControllerState':
rospy.Subscriber(
self.state_topics[i],
control_msgs.msg.JointTrajectoryControllerState,
self.state_cb_update)
elif self.state_type[
i] == 'pr2_controllers_msgs/JointTrajectoryControllerState':
rospy.Subscriber(
self.state_topics[i],
pr2_controllers_msgs.msg.JointTrajectoryControllerState,
self.state_cb_update)
else:
logging.logerr(
'wrong state topic type ' + self.state_type[i] +
'\nmust be either control_msgs/JointTrajectoryControllerState or pr2_controllers_msgs/JointTrajectoryControllerState'
)
exit()
rospy.Subscriber(self.state_topics[0],
control_msgs.msg.JointTrajectoryControllerState,
self.state_cb_publish)
self._as = actionlib.SimpleActionServer(
'/whole_body_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction,
execute_cb=self.callback,
auto_start=False)
self._as.register_preempt_callback(self.preempt_cb)
self._as.start()
logging.loginfo(u'Joint Trajector Splitter: running')
rospy.spin()
def callback(self, goal):
logging.loginfo('received goal')
self.success = True
idx = []
for joint_name_list in self.joint_names:
index_list = []
for joint_name in joint_name_list:
try:
index_list.append(
goal.trajectory.joint_names.index(joint_name))
except ValueError:
logging.logerr(
'the goal does not contain the joint ' + joint_name +
' but it is published by one of the state topics')
result = control_msgs.msg.FollowJointTrajectoryResult()
result.error_code = control_msgs.msg.FollowJointTrajectoryResult.INVALID_GOAL
self._as.set_aborted(result)
return
idx.append(index_list)
action_goals = []
for i in range(self.number_of_clients):
if self.client_type[
i] == 'control_msgs/FollowJointTrajectoryActionGoal':
action_goals.append(
control_msgs.msg.FollowJointTrajectoryGoal())
else:
action_goals.append(
pr2_controllers_msgs.msg.JointTrajectoryGoal())
goal_trajectories_points = [[] for i in range(self.number_of_clients)]
for p in goal.trajectory.points:
for i, index_list in enumerate(idx):
traj_point = trajectory_msgs.msg.JointTrajectoryPoint()
joint_pos = [p.positions[j] for j in index_list]
traj_point.positions = tuple(joint_pos)
if p.velocities:
joint_vel = [p.velocities[j] for j in index_list]
traj_point.velocities = tuple(joint_vel)
if p.accelerations:
joint_acc = [p.accelerations[j] for j in index_list]
traj_point.accelerations = tuple(joint_acc)
if p.effort:
joint_effort = [p.effort[j] for j in index_list]
traj_point.effort = tuple(joint_effort)
traj_point.time_from_start.nsecs = p.time_from_start.nsecs
traj_point.time_from_start.secs = p.time_from_start.secs
goal_trajectories_points[i].append(traj_point)
for i, a_goal in enumerate(action_goals):
a_goal.trajectory.header = goal.trajectory.header
a_goal.trajectory.joint_names = self.joint_names[i]
a_goal.trajectory.points = tuple(goal_trajectories_points[i])
logging.loginfo('send splitted goals')
for i in range(self.number_of_clients):
self.action_clients[i].send_goal(
action_goals[i],
feedback_cb=self.feedback_cb,
done_cb=done_cb(self.client_topics[i], self.action_clients,
self._as))
timeout = goal.trajectory.points[-1].time_from_start + rospy.Duration(
secs=2)
for i in range(self.number_of_clients):
start = rospy.Time.now()
finished_before_timeout = self.action_clients[i].wait_for_result(
timeout=timeout)
now = rospy.Time.now()
timeout = timeout - (now - start)
if not finished_before_timeout:
logging.logwarn("Client took to long to finish action")
self.success = False
self._as.set_aborted()
break
if self.success:
self._as.set_succeeded()
def initialize_god_map():
god_map = GodMap()
blackboard = Blackboard
blackboard.god_map = god_map
god_map.safe_set_data(identifier.rosparam, rospy.get_param(rospy.get_name()))
god_map.safe_set_data(identifier.robot_description, rospy.get_param(u'robot_description'))
path_to_data_folder = god_map.safe_get_data(identifier.data_folder)
# fix path to data folder
if not path_to_data_folder.endswith(u'/'):
path_to_data_folder += u'/'
god_map.safe_set_data(identifier.data_folder, path_to_data_folder)
# fix nWSR
nWSR = god_map.safe_get_data(identifier.nWSR)
if nWSR == u'None':
nWSR = None
god_map.safe_set_data(identifier.nWSR, nWSR)
pbw.start_pybullet(god_map.safe_get_data(identifier.gui))
while not rospy.is_shutdown():
try:
controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state',
JointTrajectoryControllerState,
timeout=5.0).joint_names
except ROSException as e:
logging.logerr(u'state topic not available')
logging.logerr(e)
else:
break
rospy.sleep(0.5)
joint_weight_symbols = process_joint_specific_params(identifier.joint_weights,
identifier.default_joint_weight_identifier, god_map)
process_joint_specific_params(identifier.collisions_distances, identifier.default_collision_distances, god_map)
joint_vel_symbols = process_joint_specific_params(identifier.joint_vel, identifier.default_joint_vel, god_map)
joint_acc_symbols = process_joint_specific_params(identifier.joint_acc, identifier.default_joint_acc, god_map)
world = PyBulletWorld(god_map.safe_get_data(identifier.gui),
blackboard.god_map.safe_get_data(identifier.data_folder))
god_map.safe_set_data(identifier.world, world)
robot = WorldObject(god_map.safe_get_data(identifier.robot_description),
None,
controlled_joints)
world.add_robot(robot, None, controlled_joints, joint_vel_symbols, joint_acc_symbols, joint_weight_symbols, True,
ignored_pairs=god_map.safe_get_data(identifier.ignored_self_collisions),
added_pairs=god_map.safe_get_data(identifier.added_self_collisions),
symengine_backend=god_map.safe_get_data(identifier.symengine_backend),
symengine_opt_level=god_map.safe_get_data(identifier.symengine_opt_level))
joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(),
identifier.joint_states,
suffix=[u'position'])
joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(),
identifier.joint_states,
suffix=[u'velocity'])
world.robot.reinitialize(joint_position_symbols.joint_map, joint_vel_symbols.joint_map)
return god_map
def dump_state_cb(self, data):
try:
path = self.get_god_map().unsafe_get_data(identifier.data_folder)
folder_name = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
folder_path = '{}{}'.format(path, folder_name)
os.mkdir(folder_path)
robot = self.unsafe_get_robot()
world = self.unsafe_get_world()
with open("{}/dump.txt".format(folder_path), u'w') as f:
tree_manager = self.get_god_map().unsafe_get_data(identifier.tree_manager) # type: TreeManager
joint_state_message = tree_manager.get_node(u'js1').lock.get()
f.write("initial_robot_joint_state_dict = ")
write_dict(to_joint_state_position_dict(joint_state_message), f)
f.write("try:\n" +
" x_joint = initial_robot_joint_state_dict[\"odom_x_joint\"]\n" +
" y_joint = initial_robot_joint_state_dict[\"odom_y_joint\"]\n" +
" z_joint = initial_robot_joint_state_dict[\"odom_z_joint\"]\n" +
" base_pose = PoseStamped()\n" +
" base_pose.header.frame_id = \"map\"\n" +
" base_pose.pose.position = Point(x_joint, y_joint, 0)\n" +
" base_pose.pose.orientation = Quaternion(*quaternion_about_axis(z_joint, [0, 0, 1]))\n" +
" zero_pose.teleport_base(base_pose)\n" +
"except:\n" +
" logging.loginfo(\'no x,y and z joint\')\n\n")
f.write("zero_pose.send_and_check_joint_goal(initial_robot_joint_state_dict)\n")
robot_base_pose = PoseStamped()
robot_base_pose.header.frame_id = 'map'
robot_base_pose.pose = robot.base_pose
f.write("map_odom_transform_dict = ")
write_dict(convert_ros_message_to_dictionary(robot_base_pose), f)
f.write("map_odom_pose_stamped = convert_dictionary_to_ros_message(\'geometry_msgs/PoseStamped\', map_odom_transform_dict)\n")
f.write("map_odom_transform = Transform()\n" +
"map_odom_transform.rotation = map_odom_pose_stamped.pose.orientation\n" +
"map_odom_transform.translation = map_odom_pose_stamped.pose.position\n\n")
f.write("set_odom_map_transform = rospy.ServiceProxy('/map_odom_transform_publisher/update_map_odom_transform', UpdateTransform)\n")
f.write("set_odom_map_transform(map_odom_transform)\n")
original_robot = URDFObject(robot.original_urdf)
link_names = robot.get_link_names()
original_link_names = original_robot.get_link_names()
attached_objects = list(set(link_names).difference(original_link_names))
for object_name in attached_objects:
parent = robot.get_parent_link_of_joint(object_name)
pose = robot.get_fk_pose(parent, object_name)
world_object = robot.get_sub_tree_at_joint(object_name)
f.write("#attach {}\n".format(object_name))
with open("{}/{}.urdf".format(folder_path, object_name), u'w') as f_urdf:
f_urdf.write(world_object.original_urdf)
f.write('with open(\'{}/{}.urdf\', \"r\") as f:\n'.format(folder_path, object_name))
f.write(" {}_urdf = f.read()\n".format(object_name))
f.write("{0}_name = \"{0}\"\n".format(object_name))
f.write("{}_pose_stamped_dict = ".format(object_name))
write_dict(convert_ros_message_to_dictionary(pose), f)
f.write("{0}_pose_stamped = convert_dictionary_to_ros_message('geometry_msgs/PoseStamped', {0}_pose_stamped_dict)\n".format(object_name))
f.write(
"zero_pose.add_urdf(name={0}_name, urdf={0}_urdf, pose={0}_pose_stamped)\n".format(object_name))
f.write(
"zero_pose.attach_existing(name={0}_name, frame_id=\'{1}\')\n".format(object_name, parent))
for object_name, world_object in world.get_objects().items(): # type: (str, WorldObject)
f.write("#add {}\n".format(object_name))
with open("{}/{}.urdf".format(folder_path, object_name), u'w') as f_urdf:
f_urdf.write(world_object.original_urdf)
f.write('with open(\'{}/{}.urdf\', \"r\") as f:\n'.format(folder_path,object_name))
f.write(" {}_urdf = f.read()\n".format(object_name))
f.write("{0}_name = \"{0}\"\n".format(object_name))
f.write("{0}_js_topic = \"{0}_js_topic\"\n".format(object_name))
f.write("{}_pose_dict = ".format(object_name))
write_dict(convert_ros_message_to_dictionary(world_object.base_pose), f)
f.write("{0}_pose = convert_dictionary_to_ros_message('geometry_msgs/Pose', {0}_pose_dict)\n".format(object_name))
f.write("{}_pose_stamped = PoseStamped()\n".format(object_name))
f.write("{0}_pose_stamped.pose = {0}_pose\n".format(object_name))
f.write("{0}_pose_stamped.header.frame_id = \"map\"\n".format(object_name))
f.write("zero_pose.add_urdf(name={0}_name, urdf={0}_urdf, pose={0}_pose_stamped, js_topic={0}_js_topic, set_js_topic=None)\n".format(object_name))
f.write("{}_joint_state = ".format(object_name))
write_dict(to_joint_state_position_dict((dict_to_joint_states(world_object.joint_state))), f)
f.write("zero_pose.set_object_joint_state({0}_name, {0}_joint_state)\n\n".format(object_name))
last_goal = self.get_god_map().unsafe_get_data(identifier.next_move_goal)
if last_goal:
f.write(u'last_goal_dict = ')
write_dict(convert_ros_message_to_dictionary(last_goal), f)
f.write("last_goal_msg = convert_dictionary_to_ros_message('giskard_msgs/MoveCmd', last_goal_dict)\n")
f.write("last_action_goal = MoveActionGoal()\n")
f.write("last_move_goal = MoveGoal()\n")
f.write("last_move_goal.cmd_seq = [last_goal_msg]\n")
f.write("last_move_goal.type = MoveGoal.PLAN_AND_EXECUTE\n")
f.write("last_action_goal.goal = last_move_goal\n")
f.write("zero_pose.send_and_check_goal(goal=last_action_goal)\n")
else:
f.write(u'#no goal\n')
logging.loginfo(u'saved dump to {}'.format(folder_path))
except:
logging.logerr('failed to dump state pls try again')
res = TriggerResponse()
res.message = 'failed to dump state pls try again'
return TriggerResponse()
res = TriggerResponse()
res.success = True
res.message = 'saved dump to {}'.format(folder_path)
return res
def initialize_god_map():
god_map = GodMap()
blackboard = Blackboard
blackboard.god_map = god_map
load_config_file()
god_map.set_data(identifier.rosparam, rospy.get_param(rospy.get_name()))
god_map.set_data(identifier.robot_description, rospy.get_param(u'robot_description'))
path_to_data_folder = god_map.get_data(identifier.data_folder)
# fix path to data folder
if not path_to_data_folder.endswith(u'/'):
path_to_data_folder += u'/'
god_map.set_data(identifier.data_folder, path_to_data_folder)
# fix nWSR
nWSR = god_map.get_data(identifier.nWSR)
if nWSR == u'None':
nWSR = None
god_map.set_data(identifier.nWSR, nWSR)
pbw.start_pybullet(god_map.get_data(identifier.gui))
while not rospy.is_shutdown():
try:
controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state',
JointTrajectoryControllerState,
timeout=5.0).joint_names
except ROSException as e:
logging.logerr(u'state topic not available')
logging.logerr(str(e))
else:
break
rospy.sleep(0.5)
joint_weight_symbols = process_joint_specific_params(identifier.joint_weight,
identifier.joint_weight_default,
identifier.joint_weight_override,
god_map)
process_joint_specific_params(identifier.self_collision_avoidance_distance,
identifier.self_collision_avoidance_default_threshold,
identifier.self_collision_avoidance_default_override,
god_map)
process_joint_specific_params(identifier.external_collision_avoidance_distance,
identifier.external_collision_avoidance_default_threshold,
identifier.external_collision_avoidance_default_override,
god_map)
# TODO add checks to test if joints listed as linear are actually linear
joint_velocity_linear_limit_symbols = process_joint_specific_params(identifier.joint_velocity_linear_limit,
identifier.joint_velocity_linear_limit_default,
identifier.joint_velocity_linear_limit_override,
god_map)
joint_velocity_angular_limit_symbols = process_joint_specific_params(identifier.joint_velocity_angular_limit,
identifier.joint_velocity_angular_limit_default,
identifier.joint_velocity_angular_limit_override,
god_map)
joint_acceleration_linear_limit_symbols = process_joint_specific_params(identifier.joint_acceleration_linear_limit,
identifier.joint_acceleration_linear_limit_default,
identifier.joint_acceleration_linear_limit_override,
god_map)
joint_acceleration_angular_limit_symbols = process_joint_specific_params(
identifier.joint_acceleration_angular_limit,
identifier.joint_acceleration_angular_limit_default,
identifier.joint_acceleration_angular_limit_override,
god_map)
world = PyBulletWorld(False, blackboard.god_map.get_data(identifier.data_folder))
god_map.set_data(identifier.world, world)
robot = WorldObject(god_map.get_data(identifier.robot_description),
None,
controlled_joints)
world.add_robot(robot, None, controlled_joints,
ignored_pairs=god_map.get_data(identifier.ignored_self_collisions),
added_pairs=god_map.get_data(identifier.added_self_collisions))
joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(),
identifier.joint_states,
suffix=[u'position'])
joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(),
identifier.joint_states,
suffix=[u'velocity'])
world.robot.update_joint_symbols(joint_position_symbols.joint_map, joint_vel_symbols.joint_map,
joint_weight_symbols,
joint_velocity_linear_limit_symbols, joint_velocity_angular_limit_symbols,
joint_acceleration_linear_limit_symbols, joint_acceleration_angular_limit_symbols)
world.robot.init_self_collision_matrix()
return god_map
def debug_print(self, np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, xdot_full=None):
import pandas as pd
lb = []
lbA = []
weights = []
xdot = []
# if xdot_full is not None:
# A_dot_x = np_A.dot(xdot_full)
for iJ, k in enumerate(self.joint_constraints_dict.keys()):
key = 'j -- ' + str(k)
lb.append(key)
weights.append(key)
xdot.append(key)
for iH, k in enumerate(self.hard_constraints_dict.keys()):
key = 'h -- ' + str(k)
lbA.append(key)
upper_bound = np_ubA[iH]
lower_bound = np_lbA[iH]
if np.sign(upper_bound) == np.sign(lower_bound):
logging.logwarn(u'{} out of bounds'.format(k))
if upper_bound > 0:
logging.logwarn(u'{} value below lower bound by {}'.format(k, lower_bound))
vel = np_ub[iH]
if abs(vel) < abs(lower_bound):
logging.logerr(u'joint vel of {} to low to get back into bound in one iteration'.format(vel))
else:
logging.logwarn(u'{} value above upper bound by {}'.format(k, abs(upper_bound)))
vel = np_lb[iH]
if abs(vel) < abs(lower_bound):
logging.logerr(u'joint vel of {} to low to get back into bound in one iteration'.format(vel))
for iS, k in enumerate(self.soft_constraints_dict.keys()):
key = 's -- ' + str(k)
lbA.append(key)
weights.append(key)
# xdot.append(key)
p_lb = pd.DataFrame(np_lb[:-len(self.soft_constraints_dict)], lb).sort_index()
p_ub = pd.DataFrame(np_ub[:-len(self.soft_constraints_dict)], lb).sort_index()
p_lbA = pd.DataFrame(np_lbA, lbA).sort_index()
# if xdot_full is not None:
# p_A_dot_x = pd.DataFrame(A_dot_x, lbA).sort_index()
p_ubA = pd.DataFrame(np_ubA, lbA).sort_index()
p_weights = pd.DataFrame(np_H.dot(np.ones(np_H.shape[0])), weights).sort_index()
if xdot_full is not None:
p_xdot = pd.DataFrame(xdot_full[:len(xdot)], xdot).sort_index()
p_A = pd.DataFrame(np_A, lbA, weights).sort_index(1).sort_index(0)
if self.lbAs is None:
self.lbAs = p_lbA
else:
self.lbAs = self.lbAs.T.append(p_lbA.T, ignore_index=True).T
# self.lbAs.T[[c for c in self.lbAs.T.columns if 'dist' in c]].plot()
# arrays = [(p_weights, u'H'),
# (p_A, u'A'),
# (p_lbA, u'lbA'),
# (p_ubA, u'ubA'),
# (p_lb, u'lb'),
# (p_ub, u'ub')]
# for a, name in arrays:
# self.check_for_nan(name, a)
# self.check_for_big_numbers(name, a)
pass
def __init__(self):
rospy.init_node('joint_trajectory_splitter')
self.action_clients = []
self.joint_names = []
self.state_topics = rospy.get_param('~state_topics', [])
self.client_topics = rospy.get_param('~client_topics', [])
self.number_of_clients = len(self.state_topics)
if self.number_of_clients != len(self.client_topics):
logging.logerr('Number of state and action topics do not match')
exit()
if self.number_of_clients == 0:
logging.logwarn('No state/action topic found')
exit()
for i in range(self.number_of_clients):
self.action_clients.append(
actionlib.SimpleActionClient(
self.client_topics[i],
control_msgs.msg.FollowJointTrajectoryAction))
self.joint_names.append(
rospy.wait_for_message(
self.state_topics[i], control_msgs.msg.
JointTrajectoryControllerState).joint_names)
self.current_controller_state = control_msgs.msg.JointTrajectoryControllerState(
)
total_number_joints = 0
for joint_name_list in self.joint_names:
total_number_joints += len(joint_name_list)
self.current_controller_state.joint_names.extend(joint_name_list)
self.current_controller_state.desired.positions = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.accelerations = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.effort = [
0 for i in range(total_number_joints)
]
self.current_controller_state.desired.velocities = [
0 for i in range(total_number_joints)
]
self.current_controller_state.actual = copy.deepcopy(
self.current_controller_state.desired)
self.current_controller_state.error = copy.deepcopy(
self.current_controller_state.desired)
self.state_pub = rospy.Publisher(
'/whole_body_controller/state',
control_msgs.msg.JointTrajectoryControllerState,
queue_size=10)
for topic in self.state_topics:
rospy.Subscriber(topic,
control_msgs.msg.JointTrajectoryControllerState,
self.state_cb_update)
rospy.Subscriber(self.state_topics[0],
control_msgs.msg.JointTrajectoryControllerState,
self.state_cb_publish)
self._as = actionlib.SimpleActionServer(
'/whole_body_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction,
execute_cb=self.callback,
auto_start=False)
self._as.register_preempt_callback(self.preempt_cb)
self._as.start()
rospy.spin()
