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 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 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 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 test_norm(self, v):
r1 = np.float(spw.norm(v))
r2 = np.linalg.norm(v)
self.assertTrue(np.isclose(r1, r2), msg='|{}|2=\n{} != {}'.format(v, r1, r2))