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 get_constraint(self):
soft_constraints = OrderedDict()
weight = self.get_input_float(self.weight)
root_T_tip = self.get_fk(self.root, self.tip)
goal_point = self.get_goal_point()
pointing_axis = self.get_pointing_axis()
goal_axis = goal_point - sw.position_of(root_T_tip)
goal_axis /= sw.norm(goal_axis) # FIXME possible /0
current_axis = root_T_tip * pointing_axis
diff = goal_axis - current_axis
soft_constraints[str(self) + u'x'] = SoftConstraint(lower=diff[0],
upper=diff[0],
weight=weight,
expression=current_axis[0])
soft_constraints[str(self) + u'y'] = SoftConstraint(lower=diff[1],
upper=diff[1],
weight=weight,
expression=current_axis[1])
soft_constraints[str(self) + u'z'] = SoftConstraint(lower=diff[2],
upper=diff[2],
weight=weight,
expression=current_axis[2])
return soft_constraints
def get_constraint(self):
# TODO integrate gain and max_speed?
soft_constraints = OrderedDict()
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
root_R_tip = sw.rotation_of(self.get_fk(self.root, self.tip))
tip_normal__tip = self.get_tip_normal_vector()
root_normal__root = self.get_root_normal_vector()
tip_normal__root = root_R_tip * tip_normal__tip
diff = root_normal__root - tip_normal__root
soft_constraints[str(self) + u'x'] = SoftConstraint(lower=diff[0],
upper=diff[0],
weight=weight,
expression=tip_normal__root[0])
soft_constraints[str(self) + u'y'] = SoftConstraint(lower=diff[1],
upper=diff[1],
weight=weight,
expression=tip_normal__root[1])
soft_constraints[str(self) + u'z'] = SoftConstraint(lower=diff[2],
upper=diff[2],
weight=weight,
expression=tip_normal__root[2])
return soft_constraints
def make_constraints(self, robot):
for eef in robot.end_effectors:
eef_frame = robot.frames[eef]
goal_pos = self.goal_eef[eef].get_position()
dist = spw.norm(spw.pos_of(eef_frame) - goal_pos)
goal_rot = self.goal_eef[eef].get_rotation()
eef_r = spw.rot_of(eef_frame)[:3, :3].reshape(9, 1)
goal_r = goal_rot[:3, :3].reshape(9, 1)
dist_r = spw.norm(eef_r - goal_r)
self._soft_constraints['align {} position'.format(
eef)] = SoftConstraint(
lower=-dist,
upper=-dist,
weight=self.goal_weights[eef].get_expression(),
expression=dist)
self._soft_constraints['align {} rotation'.format(
eef)] = SoftConstraint(
lower=-dist_r,
upper=-dist_r,
weight=self.goal_weights[eef].get_expression(),
expression=dist_r)
self._controllable_constraints = robot.joint_constraints
self._hard_constraints = robot.hard_constraints
self.update_observables(
{self.goal_weights[eef].get_symbol_str(): self.weight})
self.set_goal({eef: robot.get_eef_position2()[eef]})
def get_constraint(self):
"""
example:
name='CartesianPosition'
parameter_value_pair='{
"root": "base_footprint", #required
"tip": "r_gripper_tool_frame", #required
"goal_position": {"header":
{"stamp":
{"secs": 0,
"nsecs": 0},
"frame_id": "",
"seq": 0},
"pose": {"position":
{"y": 0.0,
"x": 0.0,
"z": 0.0},
"orientation": {"y": 0.0,
"x": 0.0,
"z": 0.0,
"w": 0.0}
}
}', #required
"weight": 1, #optional
"gain": 3, #optional -- error is multiplied with this value
"max_speed": 0.3 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit
}'
:return:
"""
goal_position = sw.position_of(self.get_goal_pose())
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
current_position = sw.position_of(self.get_fk(self.root, self.tip))
soft_constraints = OrderedDict()
trans_error_vector = goal_position - current_position
trans_error = sw.norm(trans_error_vector)
trans_scale = sw.diffable_min_fast(trans_error * gain, max_speed)
denominator = sw.if_eq_zero(trans_error, 1, trans_error)
trans_control = trans_error_vector / denominator * trans_scale
soft_constraints[str(self) + u'x'] = SoftConstraint(lower=trans_control[0],
upper=trans_control[0],
weight=weight,
expression=current_position[0])
soft_constraints[str(self) + u'y'] = SoftConstraint(lower=trans_control[1],
upper=trans_control[1],
weight=weight,
expression=current_position[1])
soft_constraints[str(self) + u'z'] = SoftConstraint(lower=trans_control[2],
upper=trans_control[2],
weight=weight,
expression=current_position[2])
return soft_constraints
def rotation_conv(goal_rotation,
current_rotation,
current_evaluated_rotation,
weights=1,
rot_gain=3,
max_rot_speed=0.5,
ns=''):
"""
Creates soft constrains which computes how current_rotation has to change to become goal_rotation.
:param goal_rotation: 4x4 symengine Matrix.
:type goal_rotation: sw.Matrix
:param current_rotation: 4x4 symengine Matrix. Describes current rotation with joint positions
:type current_rotation: sw.Matrix
:param current_evaluated_rotation: 4x4 symengine Matrix. contains the evaluated current rotation.
:type current_evaluated_rotation: sw.Matrix
:param weights: how important these constraints are
:type weights: sw.Symbol
:param rot_gain: how quickly max_rot_speed is reached.
:type rot_gain: sw.Symbol
:param max_rot_speed: maximum rotation speed in rad/s
:type max_rot_speed: sw.Symbol
:param ns: some string to make the constraint names unique
:return: contains the constraints.
:rtype: dict
"""
soft_constraints = OrderedDict()
axis, angle = sw.axis_angle_from_matrix(
(current_rotation.T * goal_rotation))
capped_angle = sw.diffable_max_fast(
sw.diffable_min_fast(rot_gain * angle, max_rot_speed), -max_rot_speed)
r_rot_control = axis * capped_angle
hack = sw.rotation_matrix_from_axis_angle([0, 0, 1], 0.0001)
axis, angle = sw.axis_angle_from_matrix(
(current_rotation.T * (current_evaluated_rotation * hack)).T)
c_aa = (axis * angle)
soft_constraints[u'align {} rotation 0'.format(ns)] = SoftConstraint(
lower=r_rot_control[0],
upper=r_rot_control[0],
weight=weights,
expression=c_aa[0])
soft_constraints[u'align {} rotation 1'.format(ns)] = SoftConstraint(
lower=r_rot_control[1],
upper=r_rot_control[1],
weight=weights,
expression=c_aa[1])
soft_constraints[u'align {} rotation 2'.format(ns)] = SoftConstraint(
lower=r_rot_control[2],
upper=r_rot_control[2],
weight=weights,
expression=c_aa[2])
return soft_constraints
def __init__(self, urdf_string):
super(TestController, self).__init__()
# Roboter aus URDF erzeugen
self.robot = Robot(urdf_string)
self.robot.parse_urdf()
# Regler fuer den Roboter anlegen
self.controller = SymEngineController(self.robot, '.controller_cache/')
# Transformation vom Greifer zur Basis des Roboters erzeugen
gripper_pose = self.robot.get_fk_expression('base_link',
'hand_palm_link')
# Parameterisierbaren Punkt erzeugen
self.point_input = Point3Input(to_expr, 'goal')
# Mathematischen Punkt vom Punkt-Input erzeugen lassen
goal_point = self.point_input.get_expression()
# Distanz zwischen Zielpunkt und Greiferposition
d = norm(goal_point - position_of(gripper_pose))
a = dot(unitX, gripper_pose * unitZ)
c_dist_angular = SoftConstraint(1 - a, 1 - a, 1, a)
# Constraint, der die Distanz Richtung 0 zwingt
c_dist = SoftConstraint(-d, -d, 1, d)
free_symbols = configure_controller(self.controller, self.robot, {
'dist': c_dist,
'dist_angular': c_dist_angular
})
# Dictionary fuer die derzeitige Variablenbelegung. Initial sind alle Variablen 0
self.cur_subs = {s: 0 for s in free_symbols}
# Publisher fuer Kommandos
self.pub_cmd = rospy.Publisher('/hsr/commands/joint_velocities',
JointStateMsg,
queue_size=1)
# Abonent fuer Zielpunkt
self.sub_point = rospy.Subscriber('/goal_point',
PointMsg,
self.cb_point,
queue_size=1)
# Abonent fuer Gelenkpositionen
self.sub_js = rospy.Subscriber('/hsr/joint_states',
JointStateMsg,
self.cb_js,
queue_size=1)
def make_constraints(self, robot):
t = time()
for eef in robot.end_effectors:
start_pose = robot.get_eef_position()[eef]
start_position = spw.pos_of(start_pose)
current_pose = robot.frames[eef]
current_position = spw.pos_of(current_pose)
current_rotation = spw.rot_of(current_pose)[:3, :3]
goal_position = self.goal_eef[eef].get_position()
goal_rotation = self.goal_eef[eef].get_rotation()[:3, :3]
# pos control
dist = spw.norm(spw.pos_of(current_pose) - goal_position)
# line
x0 = current_position[:-1, :]
x1 = spw.Matrix((start_position[:-1, :] + np.random.random(
(3, 1)) * 0.005).astype(float).tolist())
x2 = goal_position[:-1, :]
dist_to_line = spw.norm(spw.cross((x0 - x1),
(x0 - x2))) / spw.norm(x2 - x1)
dist_to_line = dist_to_line**2
# rot control
dist_r = spw.norm(
current_rotation.reshape(9, 1) - goal_rotation.reshape(9, 1))
self._soft_constraints['align {} position'.format(
eef)] = SoftConstraint(
lower=-dist,
upper=-dist,
weight=self.goal_weights[eef].get_expression(),
expression=dist)
self._soft_constraints['align {} rotation'.format(
eef)] = SoftConstraint(
lower=-dist_r,
upper=-dist_r,
weight=self.goal_weights[eef].get_expression(),
expression=dist_r)
self._soft_constraints['{} stay on line'.format(
eef)] = SoftConstraint(
lower=-dist_to_line,
upper=-dist_to_line,
weight=self.goal_weights[eef].get_expression() * 100,
expression=dist_to_line)
self._controllable_constraints = robot.joint_constraints
self._hard_constraints = robot.hard_constraints
self.update_observables(
{self.goal_weights[eef].get_symbol_str(): self.weight})
self.set_goal({eef: robot.get_eef_position2()[eef]})
print('make constraints took {}'.format(time() - t))
def position_conv(goal_position,
current_position,
weights=1,
trans_gain=3,
max_trans_speed=0.3,
ns=''):
"""
Creates soft constrains which computes how current_position has to change to become goal_position.
:param goal_position: 4x1 symengine Matrix.
:type goal_position: sw.Matrix
:param current_position: 4x1 symengine Matrix. Describes fk with joint positions.
:type current_position: sw.Matrix
:param weights: how important are these constraints
:type weights: sw.Symbol
:param trans_gain: how was max_trans_speed is reached.
:type trans_gain: sw.Symbol
:param max_trans_speed: maximum speed in m/s
:type max_trans_speed: sw.Symbol
:param ns: some string to make constraint names unique
:type ns: str
:return: contains the constraints
:rtype: dict
"""
soft_constraints = OrderedDict()
trans_error_vector = goal_position - current_position
trans_error = sw.norm(trans_error_vector)
trans_scale = sw.diffable_min_fast(trans_error * trans_gain,
max_trans_speed)
trans_control = trans_error_vector / trans_error * trans_scale
soft_constraints[u'align {} x position'.format(ns)] = SoftConstraint(
lower=trans_control[0],
upper=trans_control[0],
weight=weights,
expression=current_position[0])
soft_constraints[u'align {} y position'.format(ns)] = SoftConstraint(
lower=trans_control[1],
upper=trans_control[1],
weight=weights,
expression=current_position[1])
soft_constraints[u'align {} z position'.format(ns)] = SoftConstraint(
lower=trans_control[2],
upper=trans_control[2],
weight=weights,
expression=current_position[2])
return soft_constraints
def get_constraint(self):
"""
example:
name='JointPosition'
parameter_value_pair='{
"joint_name": "torso_lift_joint", #required
"goal_position": 0, #required
"weight": 1, #optional
"gain": 10, #optional -- error is multiplied with this value
"max_speed": 1 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit
}'
:return:
"""
current_joint = self.get_input_joint_position(self.joint_name)
joint_goal = self.get_input_float(self.goal)
weight = self.get_input_float(self.weight)
p_gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
soft_constraints = OrderedDict()
if self.get_robot().is_joint_continuous(self.joint_name):
err = sw.shortest_angular_distance(current_joint, joint_goal)
else:
err = joint_goal - current_joint
capped_err = sw.diffable_max_fast(sw.diffable_min_fast(p_gain * err, max_speed), -max_speed)
soft_constraints[str(self)] = SoftConstraint(lower=capped_err,
upper=capped_err,
weight=weight,
expression=current_joint)
return soft_constraints
def continuous_joint_position(current_joint, rotation_distance, weight, p_gain,
max_speed, constraint_name):
"""
:type current_joint: sw.Symbol
:type rotation_distance: sw.Symbol
:type weight: sw.Symbol
:type p_gain: sw.Symbol
:param max_speed: in rad/s or m/s depending on joint type.
:type max_speed: sw.Symbol
:type constraint_name: str
:dict:
"""
# TODO almost the same as joint_position
soft_constraints = OrderedDict()
capped_err = sw.diffable_max_fast(
sw.diffable_min_fast(p_gain * rotation_distance, max_speed),
-max_speed)
soft_constraints[constraint_name] = SoftConstraint(
lower=capped_err,
upper=capped_err,
weight=weight,
expression=current_joint)
# add_debug_constraint(soft_constraints, '{} //change//'.format(name), change)
# add_debug_constraint(soft_constraints, '{} //max_speed//'.format(name), max_speed)
return soft_constraints
def setUp(self):
self.obj_input = ProbabilisticObjectInput('object')
self.robot = Fetch()
t = time()
self.cga = self.cylinder_grasp_affordance(self.robot.gripper, self.obj_input)
print('cga time {}'.format(time()-t))
self.s_dict = {'soft_constraint': SoftConstraint(-1, 1, 1, self.cga)}
def get_constraint(self):
soft_constraints = OrderedDict()
a_P_pa = self.get_closest_point_on_a()
r_V_n = self.get_contact_normal_on_b()
actual_distance = self.get_actual_distance()
repel_speed = self.get_input_float(self.repel_speed)
t = self.get_input_sampling_period()
zero_weight_distance = self.get_input_float(self.zero_weight_distance)
A = self.get_input_float(self.A)
B = self.get_input_float(self.B)
C = self.get_input_float(self.C)
# a_T_r = self.get_fk_evaluated(self.joint_name, self.robot_root)
child_link = self.get_robot().get_child_link_of_joint(self.joint_name)
r_T_a = self.get_fk(self.robot_root, child_link)
# a_P_pa = w.dot(a_T_r, r_P_pa)
r_P_pa = w.dot(r_T_a, a_P_pa)
dist = w.dot(r_V_n.T, r_P_pa)[0]
weight_f = w.Max(w.Min(MAX_WEIGHT, A / (w.Max(actual_distance, 0) + C) + B), ZERO_WEIGHT)
limit = zero_weight_distance - actual_distance
limit = w.Min(w.Max(limit, -repel_speed * t), repel_speed * t)
soft_constraints[str(self)] = SoftConstraint(lower=limit,
upper=1e9,
weight=weight_f,
expression=dist)
return soft_constraints
def get_constraint(self):
soft_constraints = OrderedDict()
current_joint = self.get_input_joint_position(self.joint_name)
weight = self.get_input_float(self.weight)
parent_link = self.get_robot().get_parent_link_of_joint(self.joint_name)
parent_R_root = sw.rotation_of(self.get_fk(parent_link, self.get_robot().get_root()))
com__parent = sw.position_of(self.get_fk_evaluated(parent_link, self.object_name))
com__parent[3] = 0
com__parent = sw.scale(com__parent, 1)
g = sw.vector3(0, 0, -1)
g__parent = parent_R_root * g
axis_of_rotation = sw.vector3(*self.get_robot().get_joint_axis(self.joint_name))
l = sw.dot(g__parent, axis_of_rotation)
goal__parent = g__parent - sw.scale(axis_of_rotation, l)
goal__parent = sw.scale(goal__parent, 1)
goal_vel = sw.acos(sw.dot(com__parent, goal__parent))
ref_axis_of_rotation = sw.cross(com__parent, goal__parent)
goal_vel *= sw.sign(sw.dot(ref_axis_of_rotation, axis_of_rotation))
soft_constraints[str(self)] = SoftConstraint(lower=goal_vel, # sw.Min(goal_vel, 0),
upper=goal_vel, # sw.Max(goal_vel, 0),
weight=weight,
expression=current_joint)
return soft_constraints
def add_debug_constraint(d, key, expr):
"""
If you want to see an arbitrary evaluated expression in the matrix use this.
These softconstraints will not influence anything.
:param d: a dict where the softcontraint will be added to
:type: dict
:param key: a name to identify the debug soft contraint
:type key: str
:type expr: sw.Symbol
"""
d[key] = SoftConstraint(lower=expr, upper=expr, weight=0, expression=1)
def make_constraints(self, robot):
t = time()
for eef in robot.end_effectors:
start_pose = robot.get_eef_position2()[eef]
# start_position = spw.pos_of(start_pose)
current_pose = robot.frames[eef]
current_rotation = spw.rot_of(current_pose)[:3, :3]
goal_position = self.goal_eef[eef].get_position()
goal_rotation = self.goal_eef[eef].get_rotation()[:3, :3]
# goal_r = goal_rotation[:3,:3].reshape(9,1)
#pos control
dist = spw.norm(spw.pos_of(current_pose) - goal_position)
#rot control
dist_r = spw.norm(
current_rotation.reshape(9, 1) - goal_rotation.reshape(9, 1))
self._soft_constraints['align {} position'.format(
eef)] = SoftConstraint(
lower=-dist,
upper=-dist,
weight=self.goal_weights[eef].get_expression(),
expression=dist)
self._soft_constraints['align {} rotation'.format(
eef)] = SoftConstraint(
lower=-dist_r,
upper=-dist_r,
weight=self.goal_weights[eef].get_expression(),
expression=dist_r)
self._controllable_constraints = robot.joint_constraints
self._hard_constraints = robot.hard_constraints
self.update_observables(
{self.goal_weights[eef].get_symbol_str(): self.weight})
self.set_goal({eef: start_pose})
print('make constraints took {}'.format(time() - t))
def make_constraints(self, robot):
for eef in robot.end_effectors:
eef_frame = robot.frames[eef]
goal_expr = self.goal_eef[eef].get_expression()
# dist = norm(sp.Add(pos_of(eef_frame), - goal_expr, evaluate=False))
dist = spw.norm(spw.pos_of(eef_frame) - goal_expr)
self._soft_constraints['align {} position'.format(eef)] = SoftConstraint(lower=-dist,
upper=-dist,
weight=self.goal_weights[eef].get_expression(),
expression=dist)
self._controllable_constraints = robot.joint_constraints
self._hard_constraints = robot.hard_constraints
self.update_observables({self.goal_weights[eef].get_symbol_str(): self.weight})
self.set_goal([0,0,0], eef)
def link_to_link_avoidance(link_name,
current_pose,
current_pose_eval,
point_on_link,
other_point,
contact_normal,
lower_limit=0.05,
upper_limit=1e9,
weight=10000):
"""
Pushes a robot link away from another point.
:type link_name: str
:param current_pose: 4x4 symengine matrix describing the fk to the link with joint positions.
:type current_pose: sw.Matrix
:param current_pose_eval: 4x4 symengine matrix which contains the pose of the link. The entries should only be one symbol
which get directly replaced with the fk.
:type current_pose_eval: sw.Matrix
:param point_on_link: 4x1 symengine Matrix. Point on the link in root frame.
:type point_on_link: sw.Matrix
:param other_point: 4x1 symengine Matrix. Position of the other point in root frame.
:type other_point: sw.Matrix
:param contact_normal: 4x1 symengine Matrix. Vector pointing from the other point to the contact point on the link.
:type contact_normal: sw.Matrix
:param lower_limit: minimal allowed distance to the other point.
:type lower_limit: sw.Symbol
:param upper_limit: maximum distance allowed to the other point.
:type upper_limit: sw.Symbol
:param weight: How important this constraint is.
:type weight: sw.Symbol
:return: contains the soft constraint.
:rtype: dict
"""
soft_constraints = OrderedDict()
name = u'{} to any collision'.format(link_name)
controllable_point = current_pose * sw.inverse_frame(
current_pose_eval) * point_on_link
dist = (contact_normal.T * (controllable_point - other_point))[0]
soft_constraints[u'{} '.format(name)] = SoftConstraint(lower=lower_limit -
dist,
upper=upper_limit,
weight=weight,
expression=dist)
# add_debug_constraint(soft_constraints, '{} //debug dist//'.format(name), dist)
# add_debug_constraint(soft_constraints, '{} //debug n0//'.format(name), contact_normal[0])
# add_debug_constraint(soft_constraints, '{} //debug n1//'.format(name), contact_normal[1])
# add_debug_constraint(soft_constraints, '{} //debug n2//'.format(name), contact_normal[2])
return soft_constraints
def make_constraints(self, robot):
t = time()
default_weights = OrderedDict()
for joint_name in robot.get_joint_names():
joint_symbol = robot.get_joint_state_input().to_symbol(joint_name)
goal = self.goal_joint_states.to_symbol(joint_name)
weight = self.goal_weights.to_symbol(joint_name)
sc = SoftConstraint(lower=goal - joint_symbol,
upper=goal - joint_symbol,
weight=weight,
expression=joint_symbol)
self._soft_constraints[joint_name] = sc
default_weights[joint_name] = self.weight
self.update_observables(
self.goal_weights.get_update_dict(**default_weights))
super(JointSpaceControl, self).make_constraints(robot)
print('make constraints took {}'.format(time() - t))
def get_constraint(self):
soft_constraints = OrderedDict()
repel_speed = self.get_input_float(self.repel_speed)
t = self.get_input_sampling_period()
zero_weight_distance = self.get_input_float(self.zero_weight_distance)
actual_distance = self.get_actual_distance()
A = self.get_input_float(self.A)
B = self.get_input_float(self.B)
C = self.get_input_float(self.C)
r_T_b = self.get_fk_evaluated(self.robot_root, self.link_b)
movable_joint = self.get_robot().get_controlled_parent_joint(self.link_a)
f = self.get_robot().get_child_link_of_joint(movable_joint)
a_T_f = self.get_fk_evaluated(self.link_a, f)
b_T_a = self.get_fk(self.link_b, self.link_a)
pb_T_r = w.inverse_frame(self.get_r_T_pb())
f_P_pa = self.get_closest_point_on_a()
r_V_n = self.get_contact_normal_on_b()
pb_T_b = w.dot(pb_T_r, r_T_b)
a_P_pa = w.dot(a_T_f, f_P_pa)
pb_P_pa = w.dot(pb_T_b, b_T_a, a_P_pa)
pb_V_n = w.dot(pb_T_r, r_V_n)
dist = w.dot(pb_V_n.T, pb_P_pa)[0]
weight_f = w.Max(w.Min(MAX_WEIGHT, A / (w.Max(actual_distance, 0) + C) + B), ZERO_WEIGHT)
limit = zero_weight_distance - actual_distance
limit = w.Min(w.Max(limit, -repel_speed * t), repel_speed * t)
soft_constraints[str(self)] = SoftConstraint(lower=limit,
upper=1e9,
weight=weight_f,
expression=dist)
return soft_constraints
def get_constraint(self):
goal_position = sw.position_of(self.get_goal_pose())
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
t = self.get_input_sampling_period()
current_position = sw.position_of(self.get_fk(self.root, self.tip))
soft_constraints = OrderedDict()
trans_error_vector = goal_position - current_position
trans_error = sw.norm(trans_error_vector)
trans_scale = sw.diffable_min_fast(trans_error * gain, max_speed * t)
trans_control = sw.save_division(trans_error_vector, trans_error) * trans_scale
soft_constraints[str(self) + u'x'] = SoftConstraint(lower=trans_control[1],
upper=trans_control[1],
weight=weight,
expression=current_position[1])
return soft_constraints
def joint_position(current_joint, joint_goal, weight, p_gain, max_speed, name):
"""
:type current_joint: sw.Symbol
:type joint_goal: sw.Symbol
:type weight: sw.Symbol
:rtype: dict
"""
soft_constraints = OrderedDict()
err = joint_goal - current_joint
capped_err = sw.diffable_max_fast(
sw.diffable_min_fast(p_gain * err, max_speed), -max_speed)
soft_constraints[name] = SoftConstraint(lower=capped_err,
upper=capped_err,
weight=weight,
expression=current_joint)
# add_debug_constraint(soft_constraints, '{} //current_joint//'.format(name), current_joint)
# add_debug_constraint(soft_constraints, '{} //joint_goal//'.format(name), joint_goal)
# add_debug_constraint(soft_constraints, '{} //max_speed//'.format(name), max_speed)
return soft_constraints
def get_constraint(self):
soft_constraints = OrderedDict()
root_T_link = self.get_root_T_link_b()
chain = self.get_robot().get_chain(self.get_robot().get_root(), self.link_name, False, True, False)
for i in range(len(chain) - 1):
l1 = chain[i]
l2 = chain[i + 1]
link_in_chain = self.get_is_link_in_chain_symbol(l1)
fk_expr = self.get_fk(l1, l2)
root_T_link *= sw.if_eq_zero(link_in_chain, sw.eye(4), fk_expr)
# add_debug_constraint(soft_constraints, str(self)+'/link in chain / '+l1, link_in_chain)
point_on_link = self.get_closest_point_on_a(self.link_name)
other_point = self.get_closest_point_on_b(self.link_name)
contact_normal = self.get_contact_normal_on_b(self.link_name)
actual_distance = self.get_actual_distance(self.link_name)
repel_speed = self.get_input_float(self.repel_speed)
max_weight_distance = self.get_input_float(self.max_weight_distance)
zero_weight_distance = self.get_input_float(self.zero_weight_distance)
A = self.get_input_float(self.A)
B = self.get_input_float(self.B)
C = self.get_input_float(self.C)
controllable_point = root_T_link * point_on_link
# actually (contact_normal.T * (controllable_point- other_point))[0]
# but other_point is constant and therefore get removed during differentiation anyway
dist = (contact_normal.T * (controllable_point))[0]
weight_f = sw.Piecewise([MAX_WEIGHT, actual_distance <= max_weight_distance],
[ZERO_WEIGHT, actual_distance > zero_weight_distance],
[A / (actual_distance + C) + B, True])
soft_constraints[str(self)] = SoftConstraint(lower=repel_speed,
upper=repel_speed,
weight=weight_f,
expression=dist)
return soft_constraints
def make_constraints(self, robot): super(MyPositionController, self).make_constraints(robot) d = norm(self.position_input.get_expression() - pos_of(robot.frames[self.eef_name])) self._soft_constraints['position constraint'] = SoftConstraint(-d, -d, 1, d)
def get_constraint(self):
"""
example:
name='CartesianPosition'
parameter_value_pair='{
"root": "base_footprint", #required
"tip": "r_gripper_tool_frame", #required
"goal_position": {"header":
{"stamp":
{"secs": 0,
"nsecs": 0},
"frame_id": "",
"seq": 0},
"pose": {"position":
{"y": 0.0,
"x": 0.0,
"z": 0.0},
"orientation": {"y": 0.0,
"x": 0.0,
"z": 0.0,
"w": 0.0}
}
}', #required
"weight": 1, #optional
"gain": 3, #optional -- error is multiplied with this value
"max_speed": 0.3 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit
}'
:return:
"""
goal_rotation = w.rotation_of(self.get_goal_pose())
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
current_rotation = w.rotation_of(self.get_fk(self.root, self.tip))
current_evaluated_rotation = w.rotation_of(self.get_fk_evaluated(self.root, self.tip))
soft_constraints = OrderedDict()
axis, angle = w.diffable_axis_angle_from_matrix(w.dot(current_rotation.T, goal_rotation))
capped_angle = w.diffable_max_fast(w.diffable_min_fast(gain * angle, max_speed), -max_speed)
r_rot_control = axis * capped_angle
hack = w.rotation_matrix_from_axis_angle([0, 0, 1], 0.0001)
axis, angle = w.diffable_axis_angle_from_matrix(
w.dot(current_rotation.T, w.dot(current_evaluated_rotation, hack)).T)
c_aa = (axis * angle)
soft_constraints[str(self) + u'/0'] = SoftConstraint(lower=r_rot_control[0],
upper=r_rot_control[0],
weight=weight,
expression=c_aa[0])
soft_constraints[str(self) + u'/1'] = SoftConstraint(lower=r_rot_control[1],
upper=r_rot_control[1],
weight=weight,
expression=c_aa[1])
soft_constraints[str(self) + u'/2'] = SoftConstraint(lower=r_rot_control[2],
upper=r_rot_control[2],
weight=weight,
expression=c_aa[2])
return soft_constraints
def get_constraint(self):
"""
example:
name='CartesianPosition'
parameter_value_pair='{
"root": "base_footprint", #required
"tip": "r_gripper_tool_frame", #required
"goal_position": {"header":
{"stamp":
{"secs": 0,
"nsecs": 0},
"frame_id": "",
"seq": 0},
"pose": {"position":
{"y": 0.0,
"x": 0.0,
"z": 0.0},
"orientation": {"y": 0.0,
"x": 0.0,
"z": 0.0,
"w": 0.0}
}
}', #required
"weight": 1, #optional
"gain": 3, #optional -- error is multiplied with this value
"max_speed": 0.3 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit
}'
:return:
"""
goal_rotation = sw.rotation_of(self.get_goal_pose())
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
current_rotation = sw.rotation_of(self.get_fk(self.root, self.tip))
current_evaluated_rotation = sw.rotation_of(self.get_fk_evaluated(self.root, self.tip))
soft_constraints = OrderedDict()
angle = sw.rotation_distance(current_rotation, goal_rotation)
angle = sw.diffable_abs(angle)
capped_angle = sw.diffable_min_fast(sw.save_division(max_speed, (gain * angle)), 1)
q1 = sw.quaternion_from_matrix(current_rotation)
q2 = sw.quaternion_from_matrix(goal_rotation)
intermediate_goal = sw.diffable_slerp(q1, q2, capped_angle)
asdf = sw.quaternion_diff(q1, intermediate_goal)
axis3, angle3 = sw.axis_angle_from_quaternion(*asdf)
r_rot_control = axis3 * angle3
hack = sw.rotation_matrix_from_axis_angle([0, 0, 1], 0.0001)
axis2, angle2 = sw.diffable_axis_angle_from_matrix((current_rotation.T * (current_evaluated_rotation * hack)).T)
c_aa = (axis2 * angle2)
soft_constraints[str(self) + u'/0'] = SoftConstraint(lower=r_rot_control[0],
upper=r_rot_control[0],
weight=weight,
expression=c_aa[0])
soft_constraints[str(self) + u'/1'] = SoftConstraint(lower=r_rot_control[1],
upper=r_rot_control[1],
weight=weight,
expression=c_aa[1])
soft_constraints[str(self) + u'/2'] = SoftConstraint(lower=r_rot_control[2],
upper=r_rot_control[2],
weight=weight,
expression=c_aa[2])
return soft_constraints