def solve(self, goal_positions, goal_quats, overwrite_joints=[], overwrite_joint_values=[], prev_state=None, vel_objectives_on=True, unconstrained=True, verbose_output=False, max_iter=11, maxtime=.05, rand_start=False):
if self.vars.rotation_mode == 'relative':
self.vars.goal_quats = []
for i, q in enumerate(goal_quats):
self.vars.goal_quats.append(T.quaternion_multiply(q, self.vars.init_ee_quats[i]))
elif self.vars.rotation_mode == 'absolute':
self.vars.goal_quats = []
for i, q in enumerate(goal_quats):
self.vars.goal_quats.append(q)
self.vars.vel_objectives_on = vel_objectives_on
self.vars.unconstrained = unconstrained
# flip goal quat if necessary
for i, j in enumerate(goal_quats):
disp = np.linalg.norm(T.quaternion_disp(self.vars.prev_goal_quats[i], self.vars.goal_quats[i]))
q = self.vars.goal_quats[i]
if disp > M.pi / 2.0:
self.vars.goal_quats[i] = [-q[0], -q[1], -q[2], -q[3]]
if self.vars.position_mode == 'relative':
self.vars.goal_positions = []
for i, p in enumerate(goal_positions):
self.vars.goal_positions.append(np.array(p) + self.vars.init_ee_positions[i])
elif self.vars.position_mode == 'absolute':
self.vars.goal_positions = []
for i, p in enumerate(goal_positions):
self.vars.goal_positions.append(np.array(p))
self.overwrite_joints = overwrite_joints
self.overwrite_joint_values = overwrite_joint_values
if prev_state == None:
initSol = self.vars.xopt
else:
initSol = prev_state
if rand_start:
initSol = rand_vec(self.vars.arm.joint_limits)
self.reset(initSol)
################################################################################################################
if self.optimization_package == 'scipy':
xopt = self.groove.solve(prev_state=initSol,max_iter=max_iter,verbose_output=verbose_output)
elif self.optimization_package == 'nlopt':
xopt = self.groove.solve(prev_state=initSol,maxtime=maxtime,verbose_output=verbose_output)
else:
raise Exception('Invalid optimization package in relaxedIK. Valid inputs are [scipy] and [nlopt]. Exiting.')
################################################################################################################
for i,name in enumerate(self.vars.overwrite_joints):
index = self.vars.joint_order.index(name)
xopt[index] = self.vars.overwrite_joint_values[i]
xopt = self.filter.filter(xopt)
self.vars.relaxedIK_vars_update(xopt)
return xopt
def solve(self, goal_positions, goal_quats):
r = self.relaxedIK_subchains[0]
pos_goals = []
quat_goals = []
# for r in self.relaxedIK_subchains:
if r.vars.rotation_mode == 'relative':
# r.vars.goal_quats = []
for i, q in enumerate(goal_quats):
quat_goals.append(
T.quaternion_multiply(q, r.vars.init_ee_quats[i]))
elif r.vars.rotation_mode == 'absolute':
# r.vars.goal_quats = []
for i, q in enumerate(goal_quats):
quat_goals.append(q)
# flip goal quat if necessary
for i, j in enumerate(goal_quats):
disp = np.linalg.norm(
T.quaternion_disp(r.vars.prev_goal_quats[i],
r.vars.goal_quats[i]))
q = r.vars.goal_quats[i]
if disp > M.pi / 2.0:
quat_goals[i] = [-q[0], -q[1], -q[2], -q[3]]
if r.vars.position_mode == 'relative':
# r.vars.goal_positions = []
for i, p in enumerate(goal_positions):
pos_goals.append(np.array(p) + r.vars.init_ee_positions[i])
elif r.vars.position_mode == 'absolute':
# r.vars.goal_positions = []
for i, p in enumerate(goal_positions):
pos_goals.append(np.array(p))
# print r.vars.goal_positions
for r in self.relaxedIK_subchains:
r.vars.goal_positions = pos_goals
r.vars.goal_quats = quat_goals
# rospy.sleep(1.0)
# for t in self.threads:
# t.join()
# threads = []
#for r in self.relaxedIK_subchains:
# t = threading.Thread(target=r.run)
# threads.append(t)
# t.start()
#for t in threads:
# t.join()
curr_target_idx = self.mt_manager.solution_count + 1
move_on = False
while not move_on:
local_move_on = True
for s in self.relaxedIK_subchains:
if not curr_target_idx == s.solution_count:
local_move_on = False
move_on = local_move_on
self.mt_manager.subchains = self.mt_manager.subchains_write
xopt = glue_subchains(self.mt_manager.subchain_indices,
self.mt_manager.subchains,
self.relaxedIK_full.vars.robot.numDOF)
# xopt = self.filter.filter(xopt)
self.mt_manager.locked_x = xopt
self.mt_manager.solution_count += 1
for r in self.relaxedIK_subchains:
r.vars.prev_goal_quats = quat_goals
r.vars.prev_goal_positions = pos_goals
return xopt
def solve(self,
goal_positions,
goal_quats,
overwrite_joints=[],
overwrite_joint_values=[],
prev_state=None,
vel_objectives_on=True,
unconstrained=True,
verbose_output=False,
max_iter=11,
maxtime=.05,
rand_start=False):
if self.vars.rotation_mode == 'relative':
self.vars.goal_quats = []
for i, q in enumerate(goal_quats):
self.vars.goal_quats.append(
T.quaternion_multiply(q, self.vars.init_ee_quats[i]))
elif self.vars.rotation_mode == 'absolute':
self.vars.goal_quats = []
for i, q in enumerate(goal_quats):
self.vars.goal_quats.append(q)
self.vars.vel_objectives_on = vel_objectives_on
self.vars.unconstrained = unconstrained
# flip goal quat if necessary
for i, j in enumerate(goal_quats):
disp = np.linalg.norm(
T.quaternion_disp(self.vars.prev_goal_quats[i],
self.vars.goal_quats[i]))
q = self.vars.goal_quats[i]
if disp > M.pi / 2.0:
self.vars.goal_quats[i] = [-q[0], -q[1], -q[2], -q[3]]
if self.vars.position_mode == 'relative':
self.vars.goal_positions = []
for i, p in enumerate(goal_positions):
self.vars.goal_positions.append(
np.array(p) + self.vars.init_ee_positions[i])
elif self.vars.position_mode == 'absolute':
self.vars.goal_positions = []
for i, p in enumerate(goal_positions):
self.vars.goal_positions.append(np.array(p))
########### Joint Overwrite ###########################################
# TODO: If this is solving for only half of a robot (one arm), it should not try to optimize the arm that is already set, because it is being controlled by a gamepad
self.vars.overwrite_joints = overwrite_joints
self.vars.overwrite_joint_values = overwrite_joint_values
#######################################################################
if prev_state == None:
initSol = self.vars.xopt
else:
initSol = prev_state
if rand_start:
initSol = rand_vec(self.vars.arm.joint_limits)
self.reset(initSol)
################################################################################################################
if self.optimization_package == 'scipy':
xopt = self.groove.solve(prev_state=initSol,
max_iter=max_iter,
verbose_output=verbose_output)
elif self.optimization_package == 'nlopt':
xopt = self.groove.solve(prev_state=initSol,
maxtime=maxtime,
verbose_output=verbose_output)
else:
raise Exception(
'Invalid optimization package in relaxedIK. Valid inputs are [scipy] and [nlopt]. Exiting.'
)
################################################################################################################
########## Joint Overwrite ########################################
# If joints were overwritten, the Groove solver doesn't know that. Overwrite its solution to include the current joint state of the un-controlled arm
# This overwritten state will be preserved as the "previous solution", so future calculations can start from this actual configuration of the robot
for i, name in enumerate(self.vars.overwrite_joints):
index = self.vars.joint_order.index(name)
xopt[index] = self.vars.overwrite_joint_values[i]
##################################################################
xopt = self.filter.filter(xopt)
self.vars.relaxedIK_vars_update(xopt)
return xopt