class ProbabilisticObjectInput(object):
def __init__(self, name):
super(ProbabilisticObjectInput, self).__init__()
self.frame = FrameInput('{}{}frame'.format(
name, ControllerInputArray.separator))
self.dimensions = Vec3Input('{}{}dimensions'.format(
name, ControllerInputArray.separator))
self.probability_class = ScalarInput('P_class')
self.probability_pos = ScalarInput('P_trans')
self.probability_rot = ScalarInput('P_rot')
def get_update_dict(self, p_object):
out_dict = self.frame.get_update_dict(*p_object.frame)
out_dict.update(self.dimensions.get_update_dict(p_object.dimensions))
out_dict.update(self.probability_class.get_update_dict(
p_object.pclass))
out_dict.update(self.probability_pos.get_update_dict(p_object.ppos))
out_dict.update(self.probability_rot.get_update_dict(p_object.prot))
return out_dict
def get_frame(self):
return self.frame.get_expression()
def get_dimensions(self):
return self.dimensions.get_expression()
def get_class_probability(self):
return self.probability_class.get_expression()
def __init__(self, name):
super(ProbabilisticObjectInput, self).__init__()
self.frame = FrameInput('{}{}frame'.format(name, ControllerInputArray.separator))
self.dimensions = Vec3Input('{}{}dimensions'.format(name, ControllerInputArray.separator))
self.probability_class = ScalarInput('P_class')
self.probability_pos = ScalarInput('P_trans')
self.probability_rot = ScalarInput('P_rot')
class EEFDiffController(QPController):
def __init__(self, robot, builder_backend=None, weight=1):
self.weight = weight
super(EEFDiffController, self).__init__(robot, builder_backend)
# @profile
def add_inputs(self, robot):
self.goal_diff = FrameInput(prefix='', suffix='goal')
self.goal_weights = ScalarInput(prefix='', suffix='sc_w')
# @profile
def make_constraints(self, robot):
t = time()
eef1 = robot.end_effectors[0]
eef1_frame = robot.frames[eef1]
eef2 = robot.end_effectors[1]
eef2_frame = robot.frames[eef2]
eef_diff_pos = spw.pos_of(eef1_frame) - spw.pos_of(eef2_frame)
goal_pos = self.goal_diff.get_position()
dist = spw.norm((eef_diff_pos) - goal_pos)
eef_diff_rot = spw.rot_of(eef1_frame)[:3, :3].T * spw.rot_of(
eef2_frame)[:3, :3]
goal_rot = self.goal_diff.get_rotation()
goal_r = goal_rot[:3, :3].reshape(9, 1)
dist_r = spw.norm(eef_diff_rot.reshape(9, 1) - goal_r)
self._soft_constraints['align eefs position'] = SoftConstraint(
lower=-dist,
upper=-dist,
weight=self.goal_weights.get_expression(),
expression=dist)
self._soft_constraints['align eefs rotation'] = SoftConstraint(
lower=-dist_r,
upper=-dist_r,
weight=self.goal_weights.get_expression(),
expression=dist_r)
self._controllable_constraints = robot.joint_constraints
self._hard_constraints = robot.hard_constraints
self.update_observables(
{self.goal_weights.get_symbol_str(): self.weight})
print('make constraints took {}'.format(time() - t))
def set_goal(self, goal):
"""
dict eef_name -> goal_position
:param goal_pos:
:return:
"""
self.update_observables(self.goal_diff.get_update_dict(*goal))
def test_frame_input(self, translation_prefix, translation_suffix, rotation_prefix, rotation_suffix,
x, y, z, qx, qy, qz, qw):
gm = GodMap()
input = FrameInput(gm.to_symbol, translation_prefix, translation_suffix, rotation_prefix, rotation_suffix,
x, y, z, qx, qy, qz, qw)
x_symbol = str(input.x)
y_symbol = str(input.y)
z_symbol = str(input.z)
qx_symbol = str(input.qx)
qy_symbol = str(input.qy)
qz_symbol = str(input.qz)
qw_symbol = str(input.qw)
for e in chain(x, translation_prefix, translation_suffix):
self.assertTrue(e in x_symbol)
for e in chain(y, translation_prefix, translation_suffix):
self.assertTrue(e in y_symbol)
for e in chain(z, translation_prefix, translation_suffix):
self.assertTrue(e in z_symbol)
for e in chain(qx, rotation_prefix, rotation_suffix):
self.assertTrue(e in qx_symbol)
for e in chain(qy, rotation_prefix, rotation_suffix):
self.assertTrue(e in qy_symbol)
for e in chain(qz, rotation_prefix, rotation_suffix):
self.assertTrue(e in qz_symbol)
for e in chain(qw, rotation_prefix, rotation_suffix):
self.assertTrue(e in qw_symbol)
def add_collision_avoidance_soft_constraints(self):
"""
Adds a constraint for each link that pushed it away from its closest point.
"""
soft_constraints = {}
for link in list(self.controllable_links):
point_on_link_input = Point3Input(
self.god_map.to_symbol,
prefix=[
self._closest_point_identifier, link, u'position_on_a'
])
other_point_input = Point3Input(self.god_map.to_symbol,
prefix=[
self._closest_point_identifier,
link, u'position_on_b'
])
current_input = FrameInput(self.god_map.to_symbol,
translation_prefix=[
self._fk_identifier,
(self.root, link), u'pose',
u'position'
],
rotation_prefix=[
self._fk_identifier,
(self.root, link), u'pose',
u'orientation'
])
min_dist = self.god_map.to_symbol(
[self._closest_point_identifier, link, u'min_dist'])
contact_normal = Vector3Input(self.god_map.to_symbol,
prefix=[
self._closest_point_identifier,
link, u'contact_normal'
])
soft_constraints.update(
link_to_link_avoidance(
link,
self.get_robot().get_fk_expression(self.root, link),
current_input.get_frame(),
point_on_link_input.get_expression(),
other_point_input.get_expression(),
contact_normal.get_expression(), min_dist))
self.controller.update_soft_constraints(
soft_constraints, self.god_map.get_registered_symbols())
def cart_goal_to_soft_constraints(self, root, tip, type):
"""
:type root: str
:type tip: str
:param type: as defined in Controller msg
:type type: int
:rtype: dict
"""
# TODO split this into 2 functions, for translation and rotation
goal_input = FrameInput(self.god_map.to_symbol,
translation_prefix=[
self._goal_identifier,
str(Controller.TRANSLATION_3D),
(root, tip), u'goal_pose', u'pose',
u'position'
],
rotation_prefix=[
self._goal_identifier,
str(Controller.ROTATION_3D), (root, tip),
u'goal_pose', u'pose', u'orientation'
])
current_input = FrameInput(self.god_map.to_symbol,
translation_prefix=[
self._fk_identifier, (root, tip),
u'pose', u'position'
],
rotation_prefix=[
self._fk_identifier, (root, tip),
u'pose', u'orientation'
])
weight_key = [self._goal_identifier, str(type), (root, tip), u'weight']
weight = self.god_map.to_symbol(weight_key)
p_gain_key = [self._goal_identifier, str(type), (root, tip), u'p_gain']
p_gain = self.god_map.to_symbol(p_gain_key)
max_speed_key = [
self._goal_identifier,
str(type), (root, tip), u'max_speed'
]
max_speed = self.god_map.to_symbol(max_speed_key)
if type == Controller.TRANSLATION_3D:
return position_conv(
goal_input.get_position(),
sw.position_of(self.get_robot().get_fk_expression(root, tip)),
weights=weight,
trans_gain=p_gain,
max_trans_speed=max_speed,
ns=u'{}/{}'.format(root, tip))
elif type == Controller.ROTATION_3D:
return rotation_conv(
goal_input.get_rotation(),
sw.rotation_of(self.get_robot().get_fk_expression(root, tip)),
current_input.get_rotation(),
weights=weight,
rot_gain=p_gain,
max_rot_speed=max_speed,
ns=u'{}/{}'.format(root, tip))
return {}
def get_fk_evaluated(self, root, tip):
return FrameInput(self.get_god_map().to_symbol,
prefix=identifier.fk_np +
[(root, tip)]).get_frame()
def get_root_T_link_b(self):
return FrameInput(self.god_map.to_symbol, self.get_identifier() + [self.root_T_link_b, tuple()]).get_frame()
def add_inputs(self, robot):
self.goal_eef = {}
self.goal_weights = {}
for eef in robot.end_effectors:
self.goal_eef[eef] = FrameInput(prefix=eef, suffix='goal')
self.goal_weights[eef] = ScalarInput(prefix=eef, suffix='sc_w')
def add_inputs(self, robot):
self.goal_diff = FrameInput(prefix='', suffix='goal')
self.goal_weights = ScalarInput(prefix='', suffix='sc_w')
def __init__(self, urdf_path, base_link, eef_link, wrist_link, wps, projection_frame):
self.vis = ROSVisualizer('force_test', base_frame=base_link, plot_topic='plot')
self.wps = [Frame(Vector3(*wp[3:]), Quaternion(*list(quaternion_from_rpy(*wp[:3])))) for wp in wps]
self.wpsIdx = 0
self.base_link = base_link
self.god_map = GodMap()
self.js_prefix = 'joint_state'
self.traj_pub = rospy.Publisher('~joint_trajectory', JointTrajectoryMsg, queue_size=1, tcp_nodelay=True)
self.wrench_pub = rospy.Publisher('~wrist_force_transformed', WrenchMsg, queue_size=1, tcp_nodelay=True)
# Giskard ----------------------------
# Init controller, setup controlled joints
self.controller = Controller(res_pkg_path(urdf_path), res_pkg_path('package://pbsbtest/.controllers/'), 0.6)
self.robot = self.controller.robot
self.js_input = JointStatesInput.prefix_constructor(self.god_map.get_expr, self.robot.get_joint_names(), self.js_prefix, 'position')
self.robot.set_joint_symbol_map(self.js_input)
self.controller.set_controlled_joints(self.robot.get_joint_names())
# Get eef and sensor frame
self.sensor_frame = self.robot.get_fk_expression(base_link, wrist_link)
self.eef_frame = self.robot.get_fk_expression(base_link, eef_link)
self.eef_pos = pos_of(self.eef_frame)
# Construct motion frame
mplate_pos = pos_of(self.robot.get_fk_expression(base_link, 'arm_mounting_plate'))
self.motion_frame = frame3_rpy(pi, 0, pi * 0.5, mplate_pos)
wp_frame = self.motion_frame * FrameInput.prefix_constructor('goal/position', 'goal/quaternion', self.god_map.get_expr).get_frame()
wp_pos = pos_of(wp_frame)
# Projection frame
self.world_to_projection_frame = projection_frame
# Pre init
self.god_map.set_data(['goal'], self.wps[0])
self.tick_rate = 50
deltaT = 1.0 / self.tick_rate
js = {j: SingleJointState(j) for j in self.robot.get_joint_names()}
js['gripper_base_gripper_left_joint'] = SingleJointState('gripper_base_gripper_left_joint')
self.god_map.set_data([self.js_prefix], js)
err = norm(wp_pos - self.eef_pos)
rot_err = dot(wp_frame * unitZ, self.eef_frame * unitZ)
scons = position_conv(wp_pos, self.eef_pos) # {'align position': SoftConstraint(-err, -err, 1, err)}
scons.update(rotation_conv(rot_of(wp_frame), rot_of(self.eef_frame), rot_of(self.eef_frame).subs(self.god_map.get_expr_values())))
self.controller.init(scons, self.god_map.get_free_symbols())
print('Solving for initial state...')
# Go to initial configuration
js = run_controller_until(self.god_map, self.controller, js, self.js_prefix, [(err, 0.01), (1 - rot_err, 0.001)], deltaT)[0]
print('About to plan trajectory...')
# Generate trajectory
trajectory = OrderedDict()
traj_stamp = rospy.Duration(0.0)
section_stamps = []
for x in range(1, len(self.wps)):
print('Heading for point {}'.format(x))
self.god_map.set_data(['goal'], self.wps[x])
js, sub_traj = run_controller_until(self.god_map, self.controller, js, self.js_prefix, [(err, 0.01)], deltaT)
for time_code, position in sub_traj.items():
trajectory[traj_stamp + time_code] = position
traj_stamp += sub_traj.keys()[-1]
print('Trajectory planned! {} points over a duration of {} seconds.'.format(len(trajectory), len(trajectory) * deltaT))
traj_msg = JointTrajectoryMsg()
traj_msg.header.stamp = rospy.Time.now()
traj_msg.joint_names = trajectory.items()[0][1].keys()
for t, v in trajectory.items():
point = JointTrajectoryPointMsg()
point.positions = [v[j].position for j in traj_msg.joint_names]
point.time_from_start = t
traj_msg.points.append(point)
js_advertised = False
print('Waiting for joint state topic to be published.')
while not js_advertised and not rospy.is_shutdown():
topics = [t for t, tt in rospy.get_published_topics()]
if '/joint_states' in topics:
js_advertised = True
if not js_advertised:
print('Robot does not seem to be started. Aborting...')
return
print('Joint state has been advertised. Waiting 2 seconds for the controller...')
rospy.sleep(2.0)
print('Sending trajectory.')
self.traj_pub.publish(traj_msg)
self.tfListener = tf.TransformListener()
self.wrench_sub = rospy.Subscriber('~wrist_wrench', WrenchMsg, callback=self.transform_wrench, queue_size=1)