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