def ros(request):
try:
logging.loginfo(u'deleting tmp test folder')
# shutil.rmtree(folder_name)
except Exception:
pass
logging.loginfo(u'init ros')
rospy.init_node(u'tests')
tf_init(60)
def kill_ros():
logging.loginfo(u'shutdown ros')
rospy.signal_shutdown(u'die')
try:
logging.loginfo(u'deleting tmp test folder')
# shutil.rmtree(folder_name)
except Exception:
pass
request.addfinalizer(kill_ros)
def ros(request):
try:
logging.loginfo(u'deleting tmp test folder')
# shutil.rmtree(folder_name)
except Exception:
pass
logging.loginfo(u'init ros')
rospy.init_node(u'tests')
tf.init(60)
launch = roslaunch.scriptapi.ROSLaunch()
launch.start()
rospy.set_param('/joint_trajectory_splitter/state_topics', [
'/whole_body_controller/base/state',
'/whole_body_controller/body/state', '/refills_finger/state'
])
rospy.set_param('/joint_trajectory_splitter/client_topics', [
'/whole_body_controller/base/follow_joint_trajectory',
'/whole_body_controller/body/follow_joint_trajectory',
'/whole_body_controller/refills_finger/follow_joint_trajectory'
])
node = roslaunch.core.Node('giskardpy',
'joint_trajectory_splitter.py',
name='joint_trajectory_splitter')
joint_trajectory_splitter = launch.launch(node)
def kill_ros():
joint_trajectory_splitter.stop()
rospy.delete_param('/joint_trajectory_splitter/state_topics')
rospy.delete_param('/joint_trajectory_splitter/client_topics')
logging.loginfo(u'shutdown ros')
rospy.signal_shutdown(u'die')
try:
logging.loginfo(u'deleting tmp test folder')
# shutil.rmtree(folder_name)
except Exception:
pass
request.addfinalizer(kill_ros)
def ros(request):
try:
logging.loginfo(u'deleting tmp test folder')
# shutil.rmtree(folder_name)
except Exception:
pass
logging.loginfo(u'init ros')
rospy.init_node('tests')
tf_init(60)
# launch = roslaunch.scriptapi.ROSLaunch()
# launch.start()
# rospy.set_param('/joint_trajectory_splitter/state_topics',
# ["/hsrb/arm_trajectory_controller/state",
# "/hsrb/omni_base_controller/state",
# "/hsrb/head_trajectory_controller/state"])
# rospy.set_param('/joint_trajectory_splitter/client_topics',
# ["/hsrb/arm_trajectory_controller/follow_joint_trajectory",
# "/hsrb/omni_base_controller/follow_joint_trajectory",
# "/hsrb/head_trajectory_controller/follow_joint_trajectory"])
# node = roslaunch.core.Node('giskardpy', 'joint_trajectory_splitter.py', name='joint_trajectory_splitter')
# joint_trajectory_splitter = launch.launch(node)
def kill_ros():
#joint_trajectory_splitter.stop()
rospy.delete_param('/joint_trajectory_splitter/state_topics')
rospy.delete_param('/joint_trajectory_splitter/client_topics')
logging.loginfo(u'shutdown ros')
rospy.signal_shutdown('die')
try:
logging.loginfo(u'deleting tmp test folder')
# shutil.rmtree(folder_name)
except Exception:
pass
request.addfinalizer(kill_ros)
def add_robot(self, robot, base_pose, controlled_joints, ignored_pairs,
added_pairs):
"""
:type robot: giskardpy.world_object.WorldObject
:type controlled_joints: list
:type base_pose: PoseStamped
"""
if not isinstance(robot, WorldObject):
raise TypeError(u'only WorldObject can be added to world')
if self.has_robot():
raise RobotExistsException(u'A robot is already loaded')
if self.has_object(robot.get_name()):
raise DuplicateNameException(
u'can\'t add robot; object with name "{}" already exists'.
format(robot.get_name()))
self._robot = Robot.from_urdf_object(
urdf_object=robot,
base_pose=base_pose,
controlled_joints=controlled_joints,
path_to_data_folder=self._path_to_data_folder,
ignored_pairs=ignored_pairs,
added_pairs=added_pairs)
logging.loginfo(u'--> added {} to world'.format(robot.get_name()))
def parse_constraints(self, cmd):
"""
:type cmd: MoveCmd
:rtype: dict
"""
loginfo(u'parsing goal message')
for constraint in itertools.chain(cmd.constraints, cmd.joint_constraints, cmd.cartesian_constraints):
try:
loginfo(u'adding constraint of type: \'{}\''.format(constraint.type))
C = self.allowed_constraint_types[constraint.type]
except KeyError:
matches = ''
for s in self.allowed_constraint_types.keys():
sm = difflib.SequenceMatcher(None, str(constraint.type).lower(), s.lower())
ratio = sm.ratio()
if ratio >= 0.5:
matches = matches + s + '\n'
if matches != '':
raise UnknownConstraintException(
u'unknown constraint {}. did you mean one of these?:\n{}'.format(constraint.type, matches))
else:
available_constraints = '\n'.join([x for x in self.allowed_constraint_types.keys()]) + '\n'
raise UnknownConstraintException(
u'unknown constraint {}. available constraint types:\n{}'.format(constraint.type,
available_constraints))
try:
if hasattr(constraint, u'parameter_value_pair'):
params = json.loads(constraint.parameter_value_pair)
else:
params = convert_ros_message_to_dictionary(constraint)
del params[u'type']
c = C(self.god_map, **params)
except Exception as e:
traceback.print_exc()
doc_string = C.__init__.__doc__
error_msg = u'Initialization of "{}" constraint failed: \n {} \n'.format(C.__name__, e)
if doc_string is not None:
error_msg = error_msg + doc_string
if not isinstance(e, GiskardException):
raise ConstraintInitalizationException(error_msg)
raise e
try:
soft_constraints = c.get_constraints()
self.soft_constraints.update(soft_constraints)
except Exception as e:
traceback.print_exc()
if not isinstance(e, GiskardException):
raise ConstraintInitalizationException(e)
raise e
loginfo(u'done parsing goal message')
def main():
import sys
def usage():
logging.logdebug("Tests for kdl_parser:\n")
logging.logdebug("kdl_parser <urdfs file>")
logging.logdebug("\tLoad the URDF from file.")
logging.logdebug("kdl_parser")
logging.logdebug("\tLoad the URDF from the parameter server.")
sys.exit(1)
if len(sys.argv) > 2:
usage()
if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
usage()
if (len(sys.argv) == 1):
robot = Robot.from_parameter_server()
else:
f = file(sys.argv[1], 'r')
robot = Robot.from_xml_string(f.read())
f.close()
tree = kdl_tree_from_urdf_model(robot)
num_non_fixed_joints = 0
for j in robot.joint_map:
if robot.joint_map[j].joint_type != 'fixed':
num_non_fixed_joints += 1
logging.loginfo("URDF non-fixed joints: %d;" % num_non_fixed_joints)
logging.loginfo("KDL joints: %d" % tree.getNrOfJoints())
logging.loginfo("URDF joints: %d; KDL segments: %d" %(len(robot.joint_map),
tree.getNrOfSegments()))
import random
base_link = robot.get_root()
end_link = robot.link_map.keys()[random.randint(0, len(robot.link_map)-1)]
chain = tree.getChain(base_link, end_link)
logging.logdinfo("Root link: %s; Random end link: %s" % (base_link, end_link))
for i in range(chain.getNrOfSegments()):
logging.logdinfo(chain.getSegment(i).getName())
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
#!/usr/bin/env python
import rospy
from giskardpy import logging
from giskardpy.config_loader import load_robot_yaml
if __name__ == u'__main__':
rospy.init_node(u'giskard_robot_config_uploader')
if not rospy.is_shutdown():
try:
old_params = rospy.get_param('giskard')
config_file_name = rospy.get_param(rospy.get_name() + u'/' + ''.join(u'config'))
robot_description_dict = load_robot_yaml(config_file_name)
old_params.update(robot_description_dict)
rospy.set_param(u'giskard', robot_description_dict)
except KeyboardInterrupt:
exit()
logging.loginfo(u'uploaded config of robot')
#!/usr/bin/env python
import rospy
from giskardpy.garden import grow_tree
from giskardpy.utils import check_dependencies
from giskardpy import logging, identifier
# TODO add pytest to package xml
# TODO add transform3d to package xml
if __name__ == u'__main__':
rospy.init_node(u'giskard')
check_dependencies()
tree = grow_tree()
tree_tick_rate = 1. / rospy.get_param(
rospy.get_name() + u'/' + u'/'.join(identifier.tree_tick_rate[1:]))
sleeper = rospy.Rate(tree_tick_rate)
logging.loginfo(u'giskard is ready')
while not rospy.is_shutdown():
try:
tree.tick()
sleeper.sleep()
except KeyboardInterrupt:
break
logging.loginfo(u'\n')
#!/usr/bin/env python
import rospy
import sys
from giskardpy.python_interface import GiskardWrapper
from giskardpy import logging
if __name__ == '__main__':
rospy.init_node('add_sphere')
giskard = GiskardWrapper()
try:
name = rospy.get_param('~name')
result = giskard.add_sphere(
name=name,
size=rospy.get_param('~size', 1),
frame_id=rospy.get_param('~frame', 'map'),
position=rospy.get_param('~position', (0, 0, 0)),
orientation=rospy.get_param('~orientation', (0, 0, 0, 1)))
if result.error_codes == result.SUCCESS:
logging.loginfo('sphere \'{}\' added'.format(name))
else:
logging.logwarn('failed to add sphere \'{}\''.format(name))
except KeyError:
logging.loginfo(
'Example call: rosrun giskardpy add_sphere.py _name:=sphere _size:=1 _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]'
)
def kill_pybullet():
try:
logging.loginfo(u'deleting tmp test folder')
# shutil.rmtree(folder_name)
except Exception:
pass
def tear_down(self):
rospy.sleep(1)
logging.loginfo(u'stopping plugins')
def compile_big_ass_M(self):
t = time()
self.free_symbols = w.free_symbols(self.big_ass_M)
self.compiled_big_ass_M = w.speed_up(self.big_ass_M, self.free_symbols)
logging.loginfo(
u'compiled symbolic expressions in {:.5f}s'.format(time() - t))
def suicide(self):
if self._pybullet_id is not None:
p.removeBody(self._pybullet_id)
self._pybullet_id = None
logging.loginfo(u'<-- removed {} from pybullet'.format(
self.get_name()))
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 make_matrices(self):
"""
Turns constrains into a function that computes the matrices needed for QPOases.
"""
# TODO split this into smaller functions to increase readability
t_total = time()
# TODO cpu intensive
weights = []
lb = []
ub = []
lbA = []
ubA = []
soft_expressions = []
hard_expressions = []
for k, c in self.joint_constraints_dict.items():
weights.append(c.weight)
lb.append(c.lower)
ub.append(c.upper)
for k, c in self.hard_constraints_dict.items():
lbA.append(c.lower)
ubA.append(c.upper)
hard_expressions.append(c.expression)
for k, c in self.soft_constraints_dict.items():
weights.append(c.weight)
lbA.append(c.lower)
ubA.append(c.upper)
lb.append(-BIG_NUMBER)
ub.append(BIG_NUMBER)
assert not w.is_matrix(c.expression), u'Matrices are not allowed as soft constraint expression'
soft_expressions.append(c.expression)
self.cython_big_ass_M = w.load_compiled_function(self.path_to_functions)
self.np_g = np.zeros(len(weights))
if self.cython_big_ass_M is None:
logging.loginfo(u'new controller with {} constraints requested; compiling'.format(len(soft_expressions)))
h = len(self.hard_constraints_dict)
s = len(self.soft_constraints_dict)
c = len(self.joint_constraints_dict)
# c s 1 1
# |----------------------------------
# h | A hard | 0 | |
# | -------------------| lbA | ubA
# s | A soft |identity| |
# |-----------------------------------
#c+s| H | lb | ub
# | ----------------------------------
self.big_ass_M = w.zeros(h+s+s+c, c+s+2)
self.big_ass_M[h+s:,:-2] = w.diag(*weights)
self.lb = w.Matrix(lb)
self.ub = w.Matrix(ub)
# make A
# hard part
A_hard = w.Matrix(hard_expressions)
A_hard = w.jacobian(A_hard, self.controlled_joints)
self.big_ass_M[:h, :c] = A_hard
# soft part
A_soft = w.Matrix(soft_expressions)
t = time()
A_soft = w.jacobian(A_soft, self.controlled_joints)
logging.loginfo(u'jacobian took {}'.format(time() - t))
identity = w.eye(A_soft.shape[0])
self.big_ass_M[h:h+s, :c] = A_soft
self.big_ass_M[h:h+s, c:c+s] = identity
self.lbA = w.Matrix(lbA)
self.ubA = w.Matrix(ubA)
self.big_ass_M[:h+s, c+s] = self.lbA
self.big_ass_M[:h+s, c+s+1] = self.ubA
self.big_ass_M[h+s:, c+s] = self.lb
self.big_ass_M[h+s:, c+s+1] = self.ub
t = time()
if self.free_symbols is None:
self.free_symbols = w.free_symbols(self.big_ass_M)
self.cython_big_ass_M = w.speed_up(self.big_ass_M,
self.free_symbols)
if self.path_to_functions is not None:
# safe_compiled_function(self.cython_big_ass_M, self.path_to_functions)
logging.loginfo(u'autowrap took {}'.format(time() - t))
else:
logging.loginfo(u'controller loaded {}'.format(self.path_to_functions))
logging.loginfo(u'controller ready {}s'.format(time() - t_total))
def add_external_collision_avoidance_constraints(
self, soft_threshold_override=None):
soft_constraints = {}
number_of_repeller = self.get_god_map().get_data(
identifier.external_collision_avoidance_repeller)
number_of_repeller_eef = self.get_god_map().get_data(
identifier.external_collision_avoidance_repeller_eef)
eef_joints = self.get_robot().get_controlled_leaf_joints()
maximum_distance = self.get_god_map().get_data(
identifier.maximum_collision_threshold)
# TODO add root joint?
remaining_joints = [
joint_name for joint_name in self.get_robot().controlled_joints
if joint_name not in eef_joints
]
for joint_name in remaining_joints:
child_links = self.get_robot(
).get_directly_controllable_collision_links(joint_name)
if child_links:
for i in range(number_of_repeller):
child_link = self.get_robot().get_child_link_of_joint(
joint_name)
hard_threshold = self.get_god_map().get_data(
identifier.external_collision_avoidance_distance +
[joint_name, u'hard_threshold'])
if soft_threshold_override is not None:
soft_threshold = soft_threshold_override
else:
soft_threshold = self.get_god_map().get_data(
identifier.external_collision_avoidance_distance +
[joint_name, u'soft_threshold'])
maximum_distance = max(maximum_distance, soft_threshold)
constraint = ExternalCollisionAvoidance(
self.god_map,
child_link,
hard_threshold=hard_threshold,
soft_threshold=soft_threshold,
idx=i,
num_repeller=number_of_repeller)
soft_constraints.update(constraint.get_constraints())
for joint_name in eef_joints:
child_link = self.get_robot().get_child_link_of_joint(joint_name)
for i in range(number_of_repeller_eef):
hard_threshold = self.get_god_map().get_data(
identifier.external_collision_avoidance_distance +
[joint_name, u'hard_threshold'])
if soft_threshold_override is not None:
soft_threshold = soft_threshold_override
else:
soft_threshold = self.get_god_map().get_data(
identifier.external_collision_avoidance_distance +
[joint_name, u'soft_threshold'])
maximum_distance = max(maximum_distance, soft_threshold)
constraint = ExternalCollisionAvoidance(
self.god_map,
child_link,
hard_threshold=hard_threshold,
soft_threshold=soft_threshold,
idx=i,
num_repeller=number_of_repeller_eef)
soft_constraints.update(constraint.get_constraints())
num_external = len(soft_constraints)
loginfo('adding {} external collision avoidance constraints'.format(
num_external))
self.soft_constraints.update(soft_constraints)
self.get_god_map().set_data(identifier.maximum_collision_threshold,
maximum_distance)
#!/usr/bin/env python
import rospy
import sys
from giskardpy.python_interface import GiskardWrapper
from giskardpy import logging
if __name__ == '__main__':
rospy.init_node('add_cylinder')
giskard = GiskardWrapper()
try:
name = rospy.get_param('~name')
result = giskard.add_cylinder(name=name,
size=rospy.get_param('~size', (1, 1)),
frame_id=rospy.get_param('~frame', 'map'),
position=rospy.get_param('~position', (0, 0, 0)),
orientation=rospy.get_param('~orientation', (0, 0, 0, 1)))
if result.error_codes == result.SUCCESS:
logging.loginfo('cylinder \'{}\' added'.format(name))
else:
logging.logwarn('failed to add cylinder \'{}\''.format(name))
except KeyError:
logging.loginfo(
'Example call: rosrun giskardpy add_cylinder.py _name:=cylinder _size:=[1,1] _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]')
#!/usr/bin/env python
import rospy
from giskardpy.urdf_object import URDFObject
from geometry_msgs.msg import Pose, Point, Quaternion
from giskardpy import logging
while not rospy.has_param('/urdf_merger/urdf_sources'):
logging.loginfo('waiting for rosparam /urdf_merger/urdf_sources')
rospy.sleep(1.0)
while not rospy.has_param('/urdf_merger/robot_name'):
logging.loginfo('waiting for rosparam /urdf_merger/robot_name')
rospy.sleep(1.0)
urdf_sources = rospy.get_param('/urdf_merger/urdf_sources')
robot_name = rospy.get_param('/urdf_merger/robot_name')
merged_object = None
if rospy.has_param(urdf_sources[0]['source']):
urdf_string = rospy.get_param(urdf_sources[0]['source'])
merged_object = URDFObject(urdf_string)
else:
merged_object = URDFObject.from_urdf_file(urdf_sources[0]['source'])
for source in urdf_sources[1:]:
if rospy.has_param(source['source']):
urdf_string = rospy.get_param(source['source'])
urdf_object = URDFObject(urdf_string)
else:
urdf_object = URDFObject.from_urdf_file(source['source'])
pose = PoseStamped()
pose.header.frame_id = map_frame
if position is not None:
pose.pose.position = Point(*position)
if orientation is not None:
pose.pose.orientation = Quaternion(*quaternion_from_euler(
*orientation))
else:
pose.pose.orientation.w = 1
if path is None:
if param_name is None:
logging.logwarn('neither _param nor _path specified')
sys.exit()
else:
urdf = rospy.get_param(param_name)
else:
with open(path, 'r') as f:
urdf = f.read()
result = giskard.add_urdf(name=name,
urdf=urdf,
pose=pose,
js_topic=rospy.get_param('~js', None))
if result.error_codes == result.SUCCESS:
logging.loginfo('urdfs \'{}\' added'.format(name))
else:
logging.logwarn('failed to add urdfs \'{}\''.format(name))
except KeyError:
logging.loginfo(
'Example call: rosrun giskardpy add_urdf.py _name:=kitchen _param:=kitchen_description _js:=kitchen/joint_states _root_frame:=iai_kitchen/world _frame_id:=map'
)
def solve(self, H, g, A, lb, ub, lbA, ubA, nWSR=None):
"""
x^T*H*x + x^T*g
s.t.: lbA < A*x < ubA
and lb < x < ub
:param H: 2d diagonal weight matrix, shape = (jc (joint constraints) + sc (soft constraints)) * (jc + sc)
:type np.array
:param g: 1d zero vector of len joint constraints + soft constraints
:type np.array
:param A: 2d jacobi matrix of hc (hard constraints) and sc, shape = (hc + sc) * (number of joints)
:type np.array
:param lb: 1d vector containing lower bound of x, len = jc + sc
:type np.array
:param ub: 1d vector containing upper bound of x, len = js + sc
:type np.array
:param lbA: 1d vector containing lower bounds for the change of hc and sc, len = hc+sc
:type np.array
:param ubA: 1d vector containing upper bounds for the change of hc and sc, len = hc+sc
:type np.array
:param nWSR:
:type np.array
:return: x according to the equations above, len = joint constraints + soft constraints
:type np.array
"""
j_mask = H.sum(axis=1) != 0
s_mask = j_mask[self.j:]
h_mask = np.concatenate((np.array([True] * self.h), s_mask))
A = A[h_mask][:, j_mask].copy()
lbA = lbA[h_mask]
ubA = ubA[h_mask]
lb = lb[j_mask]
ub = ub[j_mask]
H = H[j_mask][:, j_mask]
g = np.zeros(H.shape[0])
if A.shape != self.shape:
self.started = False
self.shape = A.shape
number_of_retries = 2
while number_of_retries > 0:
if nWSR is None:
nWSR = np.array([sum(A.shape) * 2])
else:
nWSR = np.array([nWSR])
number_of_retries -= 1
if not self.started:
self.init(A.shape[1], A.shape[0])
success = self.qpProblem.init(H, g, A, lb, ub, lbA, ubA, nWSR)
if success == PyReturnValue.MAX_NWSR_REACHED:
self.started = False
raise MAX_NWSR_REACHEDException(
u'Failed to initialize QP-problem.')
else:
success = self.qpProblem.hotstart(H, g, A, lb, ub, lbA, ubA,
nWSR)
if success == PyReturnValue.MAX_NWSR_REACHED:
self.started = False
raise MAX_NWSR_REACHEDException(
u'Failed to hot start QP-problem.')
if success == PyReturnValue.SUCCESSFUL_RETURN:
self.started = True
break
elif success == PyReturnValue.NAN_IN_LB:
# TODO nans get replaced with 0 document this somewhere
# TODO might still be buggy when nan occur when the qp problem is already initialized
lb[np.isnan(lb)] = 0
nWSR = None
self.started = False
number_of_retries += 1
continue
elif success == PyReturnValue.NAN_IN_UB:
ub[np.isnan(ub)] = 0
nWSR = None
self.started = False
number_of_retries += 1
continue
elif success == PyReturnValue.NAN_IN_LBA:
lbA[np.isnan(lbA)] = 0
nWSR = None
self.started = False
number_of_retries += 1
continue
elif success == PyReturnValue.NAN_IN_UBA:
ubA[np.isnan(ubA)] = 0
nWSR = None
self.started = False
number_of_retries += 1
continue
else:
logging.loginfo(
u'{}; retrying with A rounded to 5 decimal places'.format(
self.RETURN_VALUE_DICT[success]))
r = 5
A = np.round(A, r)
nWSR = None
self.started = False
else: # if not break
self.started = False
raise QPSolverException(self.RETURN_VALUE_DICT[success])
self.qpProblem.getPrimalSolution(self.xdot_full)
return self.xdot_full
odom_x = js.position[odom_x_id]
odom_y = js.position[odom_y_id]
odom_z = js.position[odom_z_id]
odom_original_T_goal = kdl.Frame(kdl.Rotation().RotZ(odom_z),
kdl.Vector(odom_x, odom_y, 0))
map_T_goal = map_T_odom_original * odom_original_T_goal
odom_T_goal = odom_T_map * map_T_goal
js.position[odom_x_id] = odom_T_goal.p[0]
js.position[odom_y_id] = odom_T_goal.p[1]
js.position[odom_z_id] = kdl.Rotation().RotZ(odom_z).GetRot()[2]
self.joint_state_pub.publish(js)
r.sleep()
js.velocity = [0 for _ in js.velocity]
self.joint_state_pub.publish(js)
if __name__ == u'__main__':
rospy.init_node(u'traj_to_js_publisher')
try:
traj2js = TrajToJS(odom_x_joint=rospy.get_param('~odom_x_joint'),
odom_y_joint=rospy.get_param('~odom_y_joint'),
odom_z_joint=rospy.get_param('~odom_z_joint'),
odom_frame=rospy.get_param('~odom_frame'))
rospy.sleep(0.5)
logging.loginfo(u'running')
rospy.spin()
except KeyError:
logging.loginfo(
'Example call: rosrun giskardpy traj_to_joint_states.py _odom_x_joint:=odom_x_joint _odom_y_joint:=odom_y_joint _odom_z_joint:=odom_z_joint _odom_frame:=odom'
)
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 safe_compiled_function(f, file_name):
create_path(file_name)
with open(file_name, 'w') as file:
pickle.dump(f, file)
logging.loginfo(u'saved {}'.format(file_name))
def print_body_names():
logging.loginfo("".join(get_body_names()))
def fin():
state_publisher1.stop()
rospy.delete_param('/state_publisher1/joint_names')
while (state_publisher1.is_alive()):
logging.loginfo('waiting for nodes to finish')
rospy.sleep(0.2)
#!/usr/bin/env python
import rospy
from giskardpy.python_interface import GiskardWrapper
from giskardpy import logging
if __name__ == '__main__':
rospy.init_node('clear_world')
giskard = GiskardWrapper()
result = giskard.clear_world()
if result.error_codes == result.SUCCESS:
logging.loginfo('clear world')
else:
logging.logwarn('failed to clear world {}'.format(result))
def kill_pybullet():
logging.loginfo(u'shutdown pybullet')
pbw.stop_pybullet()
def kill_pybullet():
logging.loginfo(u'resetting pybullet')
pbw.clear_pybullet()
#!/usr/bin/env python
import rospy
import sys
from giskardpy.python_interface import GiskardWrapper
from giskardpy import logging
if __name__ == '__main__':
rospy.init_node('attach_object')
giskard = GiskardWrapper()
try:
name = rospy.get_param('~name')
result = giskard.attach_object(name=name,
link_frame_id=rospy.get_param('~link'))
if result.error_codes == result.SUCCESS:
logging.loginfo('existing object \'{}\' attached'.format(name))
else:
logging.logwarn('failed to add object \'{}\''.format(name))
logging.logwarn(result)
except KeyError:
logging.loginfo(
'Example call: rosrun giskardpy attach_object.py _name:=box _link:=gripper_tool_frame'
)
