def update(self):
try:
while not self.lock.empty():
updates = self.lock.get()
for object_state in updates.object_states: # type: ObjectState
object_name = object_state.object_id
if not self.get_world().has_object(object_name):
try:
world_object = WorldObject.from_object_state(
object_state)
except CorruptShapeException:
# skip objects without visual
continue
try:
self.get_world().add_object(world_object)
except pybullet.error as e:
logging.logwarn(
u'mesh \'{}\' does not exist'.format(
object_state.mesh_path))
continue
pose_in_map = transform_pose(MAP, object_state.pose).pose
self.get_world().set_object_pose(object_name, pose_in_map)
except Empty:
pass
return Status.RUNNING
def check_for_big_numbers(self, name, p_array, big=1e10):
# FIXME fails if condition is true on first entry
p_filtered = p_array.apply(lambda x: zip(x.index[abs(x) > big].tolist(), x[x > big]), 1)
p_filtered = p_filtered[p_filtered.apply(lambda x: len(x)) > 0]
if len(p_filtered) > 0:
logging.logwarn(u'{} has the following big numbers:'.format(name))
self.print_pandas_array(p_filtered)
def check_for_nan(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.logwarn(u'{} has the following nans:'.format(name))
self.print_pandas_array(p_filtered)
else:
logging.loginfo(u'no nans')
def __call__(self, state, result):
if result._type == 'pr2_controllers_msgs/JointTrajectoryResult':
return
if result.error_code != control_msgs.msg.FollowJointTrajectoryResult.SUCCESSFUL:
for client in self.action_clients:
client.cancel_goal()
self.success = False
self._as.set_aborted(result)
logging.logwarn(u'Joint Trajector Splitter: client \'{}\' failed to execute action goal \n {}'.format(self.name, result))
def terminate(self, new_status):
with self.status_lock:
self.set_status(Status.FAILURE)
try:
self.update_thread.join()
except Exception as e:
# FIXME sometimes terminate gets called without init being called
logging.logwarn(u'terminate was called before init')
self.stop_plugins()
super(PluginBehavior, self).terminate(new_status)
def disable_node(self, node_name):
"""
disables the node with the given name
:param node_name: the name of the node
:return:
"""
t = self.tree_nodes[node_name]
if t.parent is not None:
return t.parent.disable_child(t)
else:
logging.logwarn('cannot disable root node')
return False
def update(self):
trajectory = self.get_god_map().get_data(identifier.trajectory)
if trajectory:
sample_period = self.get_god_map().get_data(
identifier.sample_period)
controlled_joints = self.get_robot().controlled_joints
try:
plot_fft(trajectory, controlled_joints,
self.path_to_data_folder, sample_period,
self.joint_name)
except:
logwarn(u'failed to save trajectory pdf')
return Status.SUCCESS
def update(self):
trajectory = self.get_god_map().get_data(identifier.trajectory)
if trajectory:
sample_period = self.get_god_map().get_data(
identifier.sample_period)
controlled_joints = self.get_robot().controlled_joints
try:
plot_trajectory(trajectory, controlled_joints,
self.path_to_data_folder, sample_period,
self.order, self.velocity_threshold,
self.scaling, self.normalize_position,
self.tick_stride)
except Exception:
logwarn(u'failed to save trajectory pdf')
return Status.SUCCESS
def init_collada(self, robot):
"""
reads the controllable joints from a collada
:param robot:
"""
robot = robot.getElementsByTagName(
'kinematics_model')[0].getElementsByTagName('technique_common')[0]
for child in robot.childNodes:
if child.nodeType is child.TEXT_NODE:
continue
if child.localName == 'joint':
name = child.getAttribute('name')
if name not in self.giskard_joints:
continue
if child.getElementsByTagName('revolute'):
joint = child.getElementsByTagName('revolute')[0]
else:
logging.logwarn("Unknown joint type %s", child)
continue
if joint:
limit = joint.getElementsByTagName('limits')[0]
minval = float(
limit.getElementsByTagName('min')
[0].childNodes[0].nodeValue)
maxval = float(
limit.getElementsByTagName('max')
[0].childNodes[0].nodeValue)
if minval == maxval: # this is fixed joint
continue
self.joint_list.append(name)
joint = {
'min': minval * pi / 180.0,
'max': maxval * pi / 180.0,
'zero': 0,
'position': 0,
'velocity': 0,
'effort': 0
}
self.free_joints[name] = joint
def transform_point(target_frame, point):
"""
Transforms a pose stamped into a different target frame.
:type target_frame: str
:type point: PointStamped
:return: Transformed pose of None on loop failure
:rtype: PointStamped
"""
global tfBuffer
if tfBuffer is None:
init()
try:
transform = tfBuffer.lookup_transform(
target_frame,
point.header.frame_id, # source frame
point.header.stamp,
rospy.Duration(5.0))
new_pose = do_transform_point(point, transform)
return new_pose
except ExtrapolationException as e:
logging.logwarn(e)
def transform_vector(target_frame, vector):
"""
Transforms a pose stamped into a different target frame.
:type target_frame: Union[str, unicode]
:type vector: Vector3Stamped
:return: Transformed pose of None on loop failure
:rtype: Vector3Stamped
"""
global tfBuffer
if tfBuffer is None:
init()
try:
transform = tfBuffer.lookup_transform(
target_frame,
vector.header.frame_id, # source frame
vector.header.stamp,
rospy.Duration(5.0))
new_pose = do_transform_vector3(vector, transform)
return new_pose
except ExtrapolationException as e:
logging.logwarn(str(e))
def check_dependencies():
"""
Checks whether the dependencies specified in the dependency.txt in the root folder of giskardpy are installed. If a
dependecy is not installed a message is printed.
"""
with open(get_ros_pkg_path('giskardpy') + '/dependencies.txt') as f:
dependencies = f.readlines()
dependencies = [x.split('#')[0] for x in dependencies]
dependencies = [x.strip() for x in dependencies]
for d in dependencies:
try:
pkg_resources.require(d)
except pkg_resources.DistributionNotFound as e:
rospkg_exists(d)
except pkg_resources.VersionConflict as e:
logging.logwarn(
'found {version_f} but version {version_r} is required'.format(
version_r=str(e.req), version_f=str(e.dist)))
def rospkg_exists(name):
"""
checks whether a ros package with the given name and version exists
:param name: the name and version of the ros package in requirements format e.g. giskard_msgs<=0.1.0
:type name: str
:return: True if it exits else False
"""
name = name.replace(' ', '')
version_list = name.split(',')
version_entrqa0[1] = re.split('(==|>=|<=|<|>)', version_list[0])
package_name = version_entrqa0[1][0]
try:
m = r.get_manifest(package_name)
except Exception as e:
logging.logwarn('package {name} not found'.format(name=name))
return False
if len(version_entrqa0[1]) == 1:
return True
if not compare_version(version_entrqa0[1][2], version_entrqa0[1][1],
m.version):
logging.logwarn(
'found ROS package {installed_name}=={installed_version} but {r} is required}'
.format(installed_name=package_name,
installed_version=str(m.version),
r=name))
return False
for entry in version_list[1:]:
operator_and_version = re.split('(==|>=|<=|<|>)', entry)
if not compare_version(operator_and_version[2],
operator_and_version[1], m.version):
logging.logwarn(
'found ROS package {installed_name}=={installed_version} but {r} is required}'
.format(installed_name=package_name,
installed_version=str(m.version),
r=name))
return False
return True
#!/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 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 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:
index_list.append(
goal.trajectory.joint_names.index(joint_name))
idx.append(index_list)
action_goals = [
control_msgs.msg.FollowJointTrajectoryGoal()
for i in range(self.number_of_clients)
]
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]), self.feedback_cb
for i in range(self.number_of_clients):
self.action_clients[i].wait_for_result()
if self.success:
result = control_msgs.msg.FollowJointTrajectoryResult.SUCCESSFUL
for action_client in self.action_clients:
result = action_client.get_result()
if result:
if result.error_code != control_msgs.msg.FollowJointTrajectoryResult.SUCCESSFUL:
logging.logwarn(
u'didn\'t receive successful from {} {}'.format(
action_client.action_client.ns, result))
break
self._as.set_succeeded(result)
#!/usr/bin/env python
import rospy
import sys
from giskardpy.python_interface import GiskardWrapper
from giskardpy import logging
if __name__ == '__main__':
rospy.init_node('remove_object')
giskard = GiskardWrapper()
try:
name = rospy.get_param('~name')
except KeyError:
try:
name = sys.argv[1]
except IndexError:
logging.loginfo(
'Example call: rosrun giskardpy remove_object.py _name:=muh')
sys.exit()
result = giskard.remove_object(name)
if result.error_codes == result.SUCCESS:
logging.loginfo('{} removed'.format(name))
else:
logging.logwarn('failed to remove {} {}'.format(name, result))
def verify_collision_entries(self, collision_goals, min_dist):
for ce in collision_goals: # type: CollisionEntry
if ce.type in [
CollisionEntry.ALLOW_ALL_COLLISIONS,
CollisionEntry.AVOID_ALL_COLLISIONS
]:
logging.logwarn(
u'ALLOW_ALL_COLLISIONS and AVOID_ALL_COLLISIONS deprecated, use AVOID_COLLISIONS and'
u'ALLOW_COLLISIONS instead with ALL constant instead.')
if ce.type == CollisionEntry.ALLOW_ALL_COLLISIONS:
ce.type = CollisionEntry.ALLOW_COLLISION
else:
ce.type = CollisionEntry.AVOID_COLLISION
for ce in collision_goals: # type: CollisionEntry
if CollisionEntry.ALL in ce.robot_links and len(
ce.robot_links) != 1:
raise PhysicsWorldException(
u'ALL used in robot_links, but it\'s not the only entry')
if CollisionEntry.ALL in ce.link_bs and len(ce.link_bs) != 1:
raise PhysicsWorldException(
u'ALL used in link_bs, but it\'s not the only entry')
if ce.body_b == CollisionEntry.ALL and not self.all_link_bs(ce):
raise PhysicsWorldException(
u'if body_b == ALL, link_bs has to be ALL as well')
self.are_entries_known(collision_goals)
for ce in collision_goals:
if not ce.robot_links:
ce.robot_links = [CollisionEntry.ALL]
if not ce.link_bs:
ce.link_bs = [CollisionEntry.ALL]
for i, ce in enumerate(reversed(collision_goals)):
if self.is_avoid_all_collision(ce):
collision_goals = collision_goals[len(collision_goals) - i -
1:]
break
if self.is_allow_all_collision(ce):
collision_goals = collision_goals[len(collision_goals) - i:]
break
else:
ce = CollisionEntry()
ce.type = CollisionEntry.AVOID_COLLISION
ce.robot_links = [CollisionEntry.ALL]
ce.body_b = CollisionEntry.ALL
ce.link_bs = [CollisionEntry.ALL]
ce.min_dist = -1
collision_goals.insert(0, ce)
# split body bs
collision_goals = self.split_body_b(collision_goals)
# split robot links
collision_goals = self.robot_related_stuff(collision_goals)
# split link_bs
collision_goals = self.split_link_bs(collision_goals)
return collision_goals
#!/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_box')
giskard = GiskardWrapper()
try:
name = rospy.get_param('~name')
result = giskard.add_box(name=name,
size=rospy.get_param('~size', (1, 1, 1)),
frame_id=rospy.get_param('~frame_id', 'map'),
position=rospy.get_param('~position', (0, 0, 0)),
orientation=rospy.get_param('~orientation', (0, 0, 0, 1)))
rospy.sleep(0.5)
if result.error_codes == result.SUCCESS:
logging.loginfo('box \'{}\' added'.format(name))
else:
logging.logwarn('failed to add box \'{}\''.format(name))
except KeyError:
logging.loginfo(
'Example call: rosrun giskardpy add_box.py _name:=box _size:=[1,1,1] _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]')
#!/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]')
map_frame = rospy.get_param('~frame_id', 'map')
if root_frame is not None:
pose = lookup_pose(map_frame, root_frame)
else:
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:
#!/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'
)
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()
def cancel_all_goals(self):
logging.logwarn('Canceling all goals of connected controllers')
for client in self.action_clients:
client.cancel_all_goals()
def init_urdf(self, robot):
"""
reads the controllable joints from a urdfs
:param robot:
"""
robot = robot.getElementsByTagName('robot')[0]
# Find all non-fixed joints that are controlled by giskard
for child in robot.childNodes:
if child.nodeType is child.TEXT_NODE:
continue
if child.localName == 'joint':
jtype = child.getAttribute('type')
if jtype in ['fixed', 'floating', 'planar']:
continue
name = child.getAttribute('name')
if name not in self.giskard_joints:
continue
self.joint_list.append(name)
if jtype == 'continuous':
minval = -pi
maxval = pi
else:
try:
limit = child.getElementsByTagName('limit')[0]
minval = float(limit.getAttribute('lower'))
maxval = float(limit.getAttribute('upper'))
except:
logging.logwarn(
"%s is not fixed, nor continuous, but limits are not specified!"
% name)
continue
safety_tags = child.getElementsByTagName('safety_controller')
if self.use_small and len(safety_tags) == 1:
tag = safety_tags[0]
if tag.hasAttribute('soft_lower_limit'):
minval = max(
minval,
float(tag.getAttribute('soft_lower_limit')))
if tag.hasAttribute('soft_upper_limit'):
maxval = min(
maxval,
float(tag.getAttribute('soft_upper_limit')))
mimic_tags = child.getElementsByTagName('mimic')
if self.use_mimic and len(mimic_tags) == 1:
tag = mimic_tags[0]
entry = {'parent': tag.getAttribute('joint')}
if tag.hasAttribute('multiplier'):
entry['factor'] = float(tag.getAttribute('multiplier'))
if tag.hasAttribute('offset'):
entry['offset'] = float(tag.getAttribute('offset'))
self.dependent_joints[name] = entry
continue
if name in self.dependent_joints:
continue
if self.zeros and name in self.zeros:
zeroval = self.zeros[name]
elif minval > 0 or maxval < 0:
zeroval = (maxval + minval) / 2
else:
zeroval = 0
joint = {'min': minval, 'max': maxval, 'zero': zeroval}
#if self.pub_def_positions:
# joint['position'] = zeroval
#if self.pub_def_vels:
# joint['velocity'] = 0.0
#if self.pub_def_efforts:
# joint['effort'] = 0.0
if jtype == 'continuous':
joint['continuous'] = True
self.free_joints[name] = joint
#!/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]'
)
