def advance(self,dt): """The state machine logic of advance() should: 1. move the pen to an approach point near the start point of path 0 (The approach point should be lifted `lift_amount` distance away from the plane) 2. lower the pen to the start point of path 0 3. trace path 0, keeping the pen's local axis paralle to the plane axis 4. raise the pen by lift_amount 5. move in a straight joint space path to an approach point near path 1's start point 6. ...likewise, repeat steps 1-4 for the remaining paths 7. move back to qhome along a straight joint space path. dt is the time step. """ model = self.robot_model controller = self.robot_controller qcur = controller.configToKlampt(controller.commandedPosition()) #TODO: implement me if not controller.isMoving(): self.wait += 1 if self.wait < 50: return self.wait = 0 #done with prior motion # model.randomizeConfig() model.setConfig(normalize_config(model.getConfig(),qcur)) #this is needed to handle the funky spin joints pen_axis_ik = ik.fixed_rotation_objective(self.end_effector, local_axis=self.pen_local_axis) if self.state == "moveinit": if self.path_index >= len(self.paths): return point_ik = ik.objective(self.end_effector, local=self.end_effector.getLocalPosition(self.cur_position), world=self.paths[self.path_index].milestones[0]) ik.solve([pen_axis_ik, point_ik]) q = model.getConfig() controller.setPiecewiseLinear([dt],[controller.configFromKlampt(q)]) self.state = "writepath" self.path_progress += 1 self.cur_position = self.paths[self.path_index].milestones[0] elif self.state == "writepath": if self.path_progress < len(self.paths[self.path_index]): point_ik = ik.objective(self.end_effector, local=self.end_effector.getLocalPosition(self.cur_position), world=self.paths[self.path_index].milestones[self.path_progress]) ik.solve([pen_axis_ik, point_ik]) q = model.getConfig() controller.setPiecewiseLinear([dt],[controller.configFromKlampt(q)]) self.path_progress += 1 self.cur_position = self.paths[self.path_index].milestones[0] else: self.state = "endpath" elif self.state == "endpath": self.path_progress = 0 self.state == "moveinit" self.path_index += 1 # duration = 2 # controller.setPiecewiseLinear([duration],[controller.configFromKlampt(q)]) #setPosition does an immediate position command #self.robot_controller.setPosition(self.robot_controller.configFromKlampt(q)) return
def set_q(self, q, debug=False): torso_x, torso_y, yaw_rads = self.estimate_torso_xy_yaw_rads_from_stance( q) if not torso_x: Logger.log(("stance: end config"), "OKGREEN") return torso_z = parameters.TORSO_Z_DESIRED fl_global = self.adjust_endeff_z(self.scatter_list[q[0]][0:3]) fr_global = self.adjust_endeff_z(self.scatter_list[q[1]][0:3]) br_global = self.adjust_endeff_z(self.scatter_list[q[2]][0:3]) bl_global = self.adjust_endeff_z(self.scatter_list[q[3]][0:3]) if debug: print(" fl_global:", Logger.pp_list(fl_global)) print(" fr_global:", Logger.pp_list(fr_global)) print(" br_global:", Logger.pp_list(br_global)) print(" bl_global:", Logger.pp_list(bl_global)) f_l_r_const = ik.objective(self.fl_end_effector, R=self.get_end_effector_rotation_matrix( 1, yaw_rad=yaw_rads), t=fl_global) f_r_r_const = ik.objective(self.fr_end_effector, R=self.get_end_effector_rotation_matrix( 2, yaw_rad=yaw_rads), t=fr_global) b_l_r_const = ik.objective(self.bl_end_effector, R=self.get_end_effector_rotation_matrix( 3, yaw_rad=yaw_rads), t=bl_global) b_r_r_const = ik.objective(self.br_end_effector, R=self.get_end_effector_rotation_matrix( 4, yaw_rad=yaw_rads), t=br_global) global_torso_xyz = [torso_x, torso_y, torso_z] des_torso_rotation = self.get_torso_R_from_yaw_rad(yaw_rads) torso_obj = ik.objective(self.torso, R=des_torso_rotation, t=global_torso_xyz) bias_config = utils.nominal_config() goals = [f_l_r_const, f_r_r_const, b_l_r_const, b_r_r_const, torso_obj] s = IKSolver(self.robosimian) for goal in goals: s.add(goal) s.setBiasConfig(bias_config) res = s.solve() if not res: print("robot_poser IK Error") return False return True
def compute_initial_final(world, robot): """ Initial point is fixed and given, target is set by 1. rotate q0 a random angle between -pi/4, 3*pi/4 2. random perturb ee position by 0.3 m If IK fails, simply abandon it. Exit when 10 plans are generated. """ end_effector = robot.link(7) space = makeSpace(world, robot) # we want to the end effector to move from start to goal start = [-0.2, -0.50, 1.4] # solve IK for the start point unit_vec1 = [0.0, -0.1, 0.0] null = [0.0, 0.0, 0.0] unit_vec2 = [0.0, 0., -0.1] objective = ik.objective(end_effector, local=[[0, 0, 0], unit_vec1], world=[start, vo.madd(start, unit_vec2, 1)]) collision_check = lambda: (space.selfCollision(robot.getConfig( ))) == False and (space.envCollision(robot.getConfig()) == False) ikflag = ik.solve_global(objective, feasibilityCheck=collision_check, startRandom=True, numRestarts=500) if ikflag == False: raise Exception("IK for start point failed!") qstart = robot.getConfig() print('Feasible ik start ', robot.getConfig()) # then IK for goal qtmp = qstart[:] qtmp[1] += np.random.random( ) * np.pi + np.pi / 2 # rotation angle, pi/2 to 3pi/2 robot.setConfig(qtmp) eepos = np.array(end_effector.getWorldPosition(null)) rand_pos = eepos.copy() rand_pos[:2] += np.random.random(2) * 0.3 - 0.15 rand_pos[2] += (np.random.random() * 0.8 - 0.4 ) # this might be too large, we will see goal = rand_pos.tolist() objective = ik.objective(end_effector, local=[[0, 0, 0], unit_vec1], world=[goal, vo.madd(goal, unit_vec2, 1)]) ikflag = ik.solve_global(objective, feasibilityCheck=collision_check, startRandom=True, numRestarts=500) if ikflag == False: raise Exception("IK for goal point failed!") print("Feasible ik goal ", robot.getConfig()) qgoal = robot.getConfig() return np.array(qstart), np.array(qgoal)
def set_q(self, q, debug=False): torso_x, torso_y, yaw_rads = self.get_torso_xy_yaw(q) if not torso_x: return torso_z = parameters.TORSO_Z_DESIRED fl_global = self.replace_end_effector_z(self.scatter_list[q[0]][0:3]) fr_global = self.replace_end_effector_z(self.scatter_list[q[1]][0:3]) br_global = self.replace_end_effector_z(self.scatter_list[q[2]][0:3]) bl_global = self.replace_end_effector_z(self.scatter_list[q[3]][0:3]) if debug: print(" fl_global:", Logger.pp_list(fl_global)) print(" fr_global:", Logger.pp_list(fr_global)) print(" br_global:", Logger.pp_list(br_global)) print(" bl_global:", Logger.pp_list(bl_global)) f_l_r_const = ik.objective(self.f_l, R=self.get_desired_end_effector_rotation( 1, yaw_rads), t=fl_global) f_r_r_const = ik.objective(self.f_r, R=self.get_desired_end_effector_rotation( 2, yaw_rads), t=fr_global) b_l_r_const = ik.objective(self.b_l, R=self.get_desired_end_effector_rotation( 3, yaw_rads), t=bl_global) b_r_r_const = ik.objective(self.b_r, R=self.get_desired_end_effector_rotation( 4, yaw_rads), t=br_global) global_torso_xyz = [torso_x, torso_y, torso_z] torso_obj = ik.objective(self.torso, local=[0, 0, 0], world=global_torso_xyz) bias_config = utils.nominal_config() goals = [f_l_r_const, f_r_r_const, b_l_r_const, b_r_r_const, torso_obj] s = IKSolver(self.robosimian) for goal in goals: s.add(goal) s.setBiasConfig(bias_config) res = s.solve() if not res: return False return True
def solve_ik(robotlink, localpos, worldpos, index): """ :param robotlink: {Klampt.RobotModelLink} -- the Klampt robot link model :param localpos: {list} -- the list of points in the robot link frame :param worldpos: {list} -- the list of points in the world frame :param index: {int} -- the index number :return: robot.getConfig(): {list} -- the list of the robot configuration """ robot = robotlink.robot() space = robotcspace.RobotCSpace(robot) obj = ik.objective(robotlink, local=localpos, world=worldpos) maxIters = 100 tol = 1e-8 for i in range(1000): s = ik.solver(obj, maxIters, tol) res = s.solve() if res and not space.selfCollision() and not space.envCollision(): return robot.getConfig() else: print( "Couldn't solve IK problem in " + str(index) + "th. Or the robot exists self-collision or environment collision." ) s.sampleInitial()
def mousefunc(self,button,state,x,y): #Put your mouse handler here #the current example prints out the list of objects clicked whenever #you right click GLWidgetProgram.mousefunc(self,button,state,x,y) self.reSolve = False dragging = False if NEW_KLAMPT: dragging = self.widgetPlugin.klamptwidgetdragging else: dragging = self.draggingWidget if not dragging and button == 2 and state==0: #down clicked = self.click_world(x,y) if clicked is not None and isinstance(clicked[0],RobotModelLink): #make a new widget link, wpt = clicked lpt = se3.apply(se3.inv(link.getTransform()),wpt) self.ikIndices.append(len(self.ikWidgets)) self.ikWidgets.append(PointPoser()) self.ikWidgets[-1].set(wpt) self.widgetMaster.add(self.ikWidgets[-1]) self.ikProblem.addObjective(ik.objective(link,local=lpt,world=wpt)) GLWidgetProgram.mousefunc(self,button,state,x,y) self.refresh() return
def advance(self,dt): """Move the end effector in a circle, maintaining its orientation dt is the time step. """ model = self.robot_model controller = self.robot_controller qcur = controller.configToKlampt(controller.commandedPosition()) # TODO: implement me if not controller.isMoving(): #done with prior motion # model.randomizeConfig() # q = model.getConfig() # duration = 2 # controller.setPiecewiseLinear([duration],[controller.configFromKlampt(q)]) #setPosition does an immediate position command # self.robot_controller.setPosition(self.robot_controller.configFromKlampt(q)) pen_axis_ik = ik.fixed_rotation_objective(self.end_effector, local_axis=self.pen_local_axis) z_axis_ik = ik.fixed_rotation_objective(self.end_effector, world_axis=[0,0,1]) next_pos = self.cur_pos(self.time + dt) point_ik = ik.objective(self.end_effector, local=self.end_effector.getLocalPosition(self.cur_position), world=next_pos) ik.solve([pen_axis_ik, z_axis_ik, point_ik]) q = model.getConfig() controller.setPiecewiseLinear([dt], [controller.configFromKlampt(q)]) self.time += dt self.cur_position = next_pos q = self.normalize_config(q, qcur) model.setConfig(q) return
def mousefunc(self, button, state, x, y): #Put your mouse handler here #the current example prints out the list of objects clicked whenever #you right click GLWidgetProgram.mousefunc(self, button, state, x, y) self.reSolve = False dragging = False if NEW_KLAMPT: dragging = self.widgetPlugin.klamptwidgetdragging else: dragging = self.draggingWidget if not dragging and button == 2 and state == 0: #down clicked = self.click_world(x, y) if clicked is not None and isinstance(clicked[0], RobotModelLink): #make a new widget link, wpt = clicked lpt = se3.apply(se3.inv(link.getTransform()), wpt) self.ikIndices.append(len(self.ikWidgets)) self.ikWidgets.append(PointPoser()) self.ikWidgets[-1].set(wpt) self.widgetMaster.add(self.ikWidgets[-1]) self.ikProblem.addObjective( ik.objective(link, local=lpt, world=wpt)) GLWidgetProgram.mousefunc(self, button, state, x, y) self.refresh() return
def set_pose(self, r_pose): torso_xyz_yawdeg = r_pose.torso_xyz_yawdeg torso_x, torso_y, torso_z, torso_yaw_deg = ( torso_xyz_yawdeg[0], torso_xyz_yawdeg[1], torso_xyz_yawdeg[2], torso_xyz_yawdeg[3], ) fl_xyz, fr_xyz, br_xyz, bl_xyz = r_pose.fl, r_pose.fr, r_pose.br, r_pose.bl yaw_rads = np.deg2rad(torso_yaw_deg) f_l_rotation = self.get_end_effector_rotation_matrix(1) f_r_rotation = self.get_end_effector_rotation_matrix(2) b_r_rotation = self.get_end_effector_rotation_matrix(3) b_l_rotation = self.get_end_effector_rotation_matrix(4) f_l_r_const = ik.objective(self.fl_end_effector, R=f_l_rotation, t=fl_xyz[0:3]) f_r_r_const = ik.objective(self.fr_end_effector, R=f_r_rotation, t=fr_xyz[0:3]) b_l_r_const = ik.objective(self.bl_end_effector, R=b_r_rotation, t=bl_xyz[0:3]) b_r_r_const = ik.objective(self.br_end_effector, R=b_l_rotation, t=br_xyz[0:3]) global_torso_xyz = [torso_x, torso_y, torso_z] des_torso_rotation = self.get_torso_R_from_yaw_rad(yaw_rads) torso_obj = ik.objective(self.torso, R=des_torso_rotation, t=global_torso_xyz) goals = [f_l_r_const, f_r_r_const, b_l_r_const, b_r_r_const, torso_obj] s = IKSolver(self.robosimian) for goal in goals: s.add(goal) # s.setTolerance(ik_max_deviation) s.setBiasConfig(utils.nominal_config()) res = s.solve() if not res: print("set_pose IK error") return False return True
def set_pose_w_R(self, r_pose): torso_xyz_yawdeg = r_pose.torso_xyz_yawdeg fl_xyz, fr_xyz, br_xyz, bl_xyz = r_pose.fl, r_pose.fr, r_pose.br, r_pose.bl fl_obj = ik.objective(self.fl_end_effector, R=fl_xyz[3], t=fl_xyz[0:3]) fr_obj = ik.objective(self.fr_end_effector, R=fr_xyz[3], t=fr_xyz[0:3]) bl_obj = ik.objective(self.bl_end_effector, R=bl_xyz[3], t=bl_xyz[0:3]) br_obj = ik.objective(self.br_end_effector, R=br_xyz[3], t=br_xyz[0:3]) des_torso_rotation = self.get_torso_R_from_yaw_rad( np.deg2rad(torso_xyz_yawdeg[3])) torso_obj = ik.objective(self.torso, R=des_torso_rotation, t=torso_xyz_yawdeg[0:3]) bias_config = utils.nominal_config() bias_config[0] = torso_xyz_yawdeg[0] bias_config[1] = torso_xyz_yawdeg[1] bias_config[2] = torso_xyz_yawdeg[2] bias_config[3] = np.deg2rad(torso_xyz_yawdeg[3]) self.robosimian.setConfig(bias_config) return ik.solve_global([torso_obj, fl_obj, fr_obj, bl_obj, br_obj])
def keyboardfunc(self, c, x, y): #Put your keyboard handler here #the current example prints the config when [space] is pressed if c == 'h' or c == '?': print("Controls:") print("- [space]: print the widget's sub-robot configuration") print("- r: randomize the sub-robot configuration") print("- p: plan to the widget's sub-robot configuration") print("- i: test the IK functions") elif c == ' ': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) print("Config:", subconfig) self.subrobots[0].setConfig(subconfig) elif c == 'r': self.subrobots[0].randomizeConfig() self.robotWidget.set(self.robot.getConfig()) elif c == 'p': config = self.robotWidget.get() subconfig = self.subrobots[0].fromfull(config) self.subrobots[0].setConfig(self.subrobots[0].fromfull( self.startConfig)) settings = { 'type': "sbl", 'perturbationRadius': 0.5, 'bidirectional': 1, 'shortcut': 1, 'restart': 1, 'restartTermCond': "{foundSolution:1,maxIters:1000}" } plan = robotplanning.planToConfig(self.world, self.subrobots[0], subconfig, movingSubset='all', **settings) plan.planMore(1000) print(plan.getPath()) elif c == 'i': link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1) print("IK solve for ee to go 10cm upward...") obj = ik.objective(link, local=[0, 0, 0], world=vectorops.add( link.getWorldPosition([0, 0, 0]), [0, 0, 0.1])) solver = ik.solver(obj) res = solver.solve() print(" result", res) print(" residual", solver.getResidual()) print(self.robotWidget.set(self.robot.getConfig())) else: GLWidgetPlugin.keyboardfunc(self, c, x, y)
def set_end_effectors_xyz_torso_xyz_yaw(self, fl, fr, br, bl, torso_x, torso_y, torso_z, yaw_rads): global_torso_xyz = [torso_x, torso_y, torso_z] des_torso_rotation = self.get_torso_R_from_yaw(yaw_rads) torso_obj = ik.objective(self.torso, R=des_torso_rotation, t=global_torso_xyz) f_l_r_const = ik.objective(self.f_l, R=self.get_desired_end_effector_rotation( 1, yaw_rads), t=fl) f_r_r_const = ik.objective(self.f_r, R=self.get_desired_end_effector_rotation( 2, yaw_rads), t=fr) b_l_r_const = ik.objective(self.b_l, R=self.get_desired_end_effector_rotation( 3, yaw_rads), t=bl) b_r_r_const = ik.objective(self.b_r, R=self.get_desired_end_effector_rotation( 4, yaw_rads), t=br) bias_config = utils.nominal_config() goals = [f_l_r_const, f_r_r_const, b_l_r_const, b_r_r_const, torso_obj] s = IKSolver(self.robosimian) for goal in goals: s.add(goal) s.setBiasConfig(bias_config) res = s.solve() if not res: print("robot_poser IK Error") return False return True
def solve(self, target_loc): if isinstance(target_loc, np.ndarray): target_loc = list(target_loc.flat) if target_loc[0] < 0: initial_cfg = self.initial_cfg_left else: initial_cfg = self.initial_cfg_right self.robot.setConfig(initial_cfg) objective = ik.objective(self.grasptarget_link, local=[0, 0, 0], world=target_loc) flag = ik.solve(objective, iters=1000, tol=1e-4) cfg = self.robot.getConfig()[7:14] return flag, cfg, initial_cfg[7:14]
def ik(self, from_config, to_ee_loc, return_flag=False): if isinstance(to_ee_loc, np.ndarray): to_ee_loc = list(to_ee_loc.flat) from_config_full = copy(self.robot.getConfig()) from_config_full[7:14] = from_config self.robot.setConfig(from_config_full) objective = ik.objective(self.grasptarget_link, local=[0, 0, 0], world=to_ee_loc) flag = ik.solve(objective, iters=1000, tol=1e-4) cfg = self.robot.getConfig()[7:14] if not return_flag: return cfg else: return cfg, flag
def retract(robot,gripper,amount,local=True): """Retracts the robot's gripper by a vector `amount`. if local=True, amount is given in local coordinates. Otherwise, its given in world coordinates. """ if not isinstance(gripper,(int,str)): gripper = gripper.base_link link = robot.link(gripper) Tcur = link.getTransform() if local: amount = so3.apply(Tcur[0],amount) obj = ik.objective(link,R=Tcur[0],t=vectorops.add(Tcur[1],amount)) res = ik.solve(obj) if not res: return None return robot.getConfig()
def robot_move(mode,world,robot,link,point_ee,point_world,maxDev,IKErrorTolerence, EEZLimit,collider,robotControlApi=None,ServoTime=9999.0,dt=1.0, use_const = True,vis=vis,use_collision_detect = False,use_ik_detect = False): robotCurrentConfig=klampt_2_controller(robot.getConfig()) goal=ik.objective(link,local=point_ee,world=point_world) res=ik.solve_nearby(goal,maxDeviation=maxDev,tol=0.00001) #res=ik.solve_global(goal,tol=0.00001) if res: # collision detect if check_collision_linear(robot,collider,controller_2_klampt(robot,robotCurrentConfig),robot.getConfig(),10): print "[!]Warning: collision detected!" if use_collision_detect == True: vis.show() if input('continue?') != 1: exit() else: pass # cal difference diff=np.max(np.absolute((np.array(vectorops.sub(robotCurrentConfig[0:5],klampt_2_controller(robot.getConfig())[0:5]))))) EEZPos=link.getTransform()[1] if diff<IKErrorTolerence and EEZPos>EEZLimit: #126 degrees if mode == 'debugging': pass elif mode == 'physical': if use_const: constantVServo(robotControlApi,ServoTime,klampt_2_controller(robot.getConfig()),dt) else: robotControlApi.setConfig(klampt_2_controller(robot.getConfig())) else: print "[!]IK too far away" if use_ik_detect == True: if input('continue?') != 1: exit() else: diff = 9999.0 print "[!]IK failture" if use_ik_detect == True: vis.show() if input('continue?') != 1: exit() return robot, diff
def solve(self, target_loc): if isinstance(target_loc, np.ndarray): target_loc = list(target_loc.flat) if target_loc[0] < 0: initial_cfg = self.initial_cfg_left else: initial_cfg = self.initial_cfg_right self.robot.setConfig(initial_cfg) # return True, self.robot.getConfig()[7:14] objective = ik.objective(self.grasptarget_link, local=[0, 0, 0], world=target_loc) flag = ik.solve(objective, iters=1000, tol=1e-4) cfg = self.robot.getConfig()[7:14] # collisions = list(self.collider.robotSelfCollisions(self.robot)) # print(f'get {len(collisions)} collisions') # for c1, c2 in collisions: # print('collisions', c1.getName(), c2.getName()) return flag, cfg, initial_cfg[7:14]
def configSolver(cur_target): global robot end_link = robot.link(robot.numLinks() - 1) ee_local_axis = end_link.getAxis() ee_local_position = (0, 0, 0) cur_target_axis = (0, 0, -1) goal = ik.objective( end_link, local=[ ee_local_position, vectorops.add(ee_local_position, ee_local_axis) ], # match end points world=[cur_target, vectorops.add(cur_target, cur_target_axis)]) ik.solve_global(goal) # ### Test Print # print('Local Coord: ', [ee_local_position, vectorops.add(ee_local_position, (0,0,-0.01))]) # print('Global Coord: ', [cur_target, vectorops.add(cur_target, (0,0,0.01))]) # ### return robot.getConfig()
def calculate_workspace_axis(robot,obstacles,end_effector,point_local,axis_local,axis_world): global lower_corner,upper_corner resolution = (15,15,15) cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution) invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner)) reachable = np.zeros(resolution) #TODO: your code here for i in range(resolution[0]): for j in range(resolution[1]): for k in range(resolution[2]): orig_config = robot.getConfig() point = vectorops.add(vectorops.mul([i,j,k], cellsize), lower_corner) world_constraint = ik.fixed_rotation_objective(end_effector, world_axis=axis_world) local_constraint = ik.fixed_rotation_objective(end_effector, local_axis=axis_local) point_goal = ik.objective(end_effector, local=point_local, world=point) if ik.solve_global([point_goal, local_constraint, world_constraint], iters=10): reachable[i,j,k] = 1.0 robot.setConfig(orig_config) return reachable
def configSolver(self, robot, catch_location): ''' solve for all the possibilities and find the one with lowest cost compare with the current config ''' # Pre-calculated waiting and prepare configs if tuple(catch_location) in PRE_CAL_CONFIG.keys(): return [PRE_CAL_CONFIG[tuple(catch_location)], 0.0] current_config = robot.getConfig() opt_sol = [current_config, float('inf')] # norm solver for link_idx in range(4, robot.numLinks() - 1): cur_end_link = robot.link(link_idx) for link_pos in [0.02 * mul for mul in range(1)]: cur_goal = ik.objective(cur_end_link, local=[(link_pos, 0, 0)], world=[catch_location]) if ik.solve(cur_goal): cur_sol = robot.getConfig() cur_dist = robot.distance(cur_sol, current_config) if cur_dist < opt_sol[1]: opt_sol[0], opt_sol[1] = cur_sol, cur_dist return opt_sol
def ik_solver(self, robot, x, q): # check over all the links in the for i in range(5): # Objective: end effector on center of target, end of final link along target axis objs = ik.objective(robot.link(6 - i), local=[(0, 0, 0)], world=[x]) s = model.ik.solver(objs) # checks for solution with 100 potential tries (Restarts) numRestarts = 100 solved = False for i in range(numRestarts): robot.setConfig(q) ## GLOBAL IMPROVEMENTS #robot.setConfig(qglobal); res = s.solve() ##now solve if (res): solved = True # print "IK success!" ## EFFECIENCY TRACKING #suc = suc + 1; #print "success:", suc, "total:", tot; ## now the robot model's configuration is changed, and you need to ## extract it. Your errpos and erraxis values should be very small ## Get world axis for end effector qseed = robot.getConfig() ## GLOBAL IMPROVEMENTS #qglobal = qseed; return (qseed, solved) # if never solved, print failure # print "IK failure... returning best solution found" return (q, solved)
def ik_solve(target_position, first_time=False): left_ee_link = robot.link(13) left_active_dofs = [7, 8, 9, 10, 11, 12] ee_local_pos = [0, 0, 0] h = 0.1 local = [ee_local_pos, vectorops.madd(ee_local_pos, (1, 0, 0), -h)] world = [target_position, vectorops.madd(target_position, (0, 0, 1), h)] goal = ik.objective(left_ee_link, local=local, world=world) if first_time: solved = ik.solve_global(goal, activeDofs=left_active_dofs, startRandom=True) else: solved = ik.solve(goal, activeDofs=left_active_dofs) if solved: print("solve success") return True else: print("Solve unsuccessful") return False
grasp_plate = grasp_plate.get_transformed(plate.getTransform()).transfer( source_gripper, target_gripper) end_effector = robot.link('EndEffector_Link') h = 0.1 ee_local_pos = (0, 0, 0) # we want the end effector to grasp from top of the object ee_local_axis = source_gripper.primary_axis # (1, 0, 0) point_local_array = np.array(ee_local_pos) axis_local_array = np.array(ee_local_axis) axis_world_array = np.array([0, 0, -1]) # grasp for swab goal_pos_world = (0.5, -0.175, 0.71 + 0.17) grasp_swab.ik_constraint = ik.objective( end_effector, local=[ee_local_pos, list(point_local_array + h * axis_local_array)], world=[goal_pos_world, list(goal_pos_world + h * axis_world_array)]) # grasp for plate goal_pos_world_plate = (0.68, 0.35, 0.87) vis.add('', goal_pos_world_plate, color=[1, 0, 0, 1]) end_effector2 = robot2.link('EndEffector_Link') grasp_plate.ik_constraint = ik.objective( end_effector2, local=[ee_local_pos, list(point_local_array + h * axis_local_array)], world=[ goal_pos_world_plate, list(goal_pos_world_plate + h * axis_world_array) ])
def solve_qplace(self, Tplacement): if self.robot0: v = list(np.array(Tplacement[1]) - np.array([0.16, 0, 0])) objective = ik.objective(self.robot.link(9), R=Tplacement[0], t=v) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('qplace') return None qpreplace = self.robot.getConfig() # add a waypoint to insert swab into human face face_trans = [0.34, -0.3, 1.05] # [0.5, -0.35, 1.35] objective = ik.objective(self.robot.link(9), R=so3.from_rpy((np.pi, -np.pi / 2, 0)), t=face_trans) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('cannot place swab near mouth') return None self.qmouth = self.robot.getConfig() self.qmouth = self.gripper.set_finger_config( self.qmouth, self.gripper.partway_open_config(self.close_amount)) face_trans = [0.34, -0.4, 1.05] # [0.5, -0.35, 1.35] objective = ik.objective(self.robot.link(9), R=so3.from_rpy((np.pi, -np.pi / 2, 0)), t=face_trans) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7]) if not solved: print('cannot insert swab into mouth') return None self.qmouth_in = self.robot.getConfig() self.qmouth_in = self.gripper.set_finger_config( self.qmouth_in, self.gripper.partway_open_config(self.close_amount)) # add a waypoint to lower down the gripper v1 = list(np.array(v) - np.array([0, 0, 0.1])) # vis.add('v1', v1, color=[0, 0, 1, 1]) objective = ik.objective(self.robot.link(9), R=so3.from_rpy( (-np.pi / 2, 0, -np.pi / 2)), t=v1) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('qlower') return None self.qlower = self.robot.getConfig() self.qlower = self.gripper.set_finger_config( self.qlower, self.gripper.partway_open_config(self.close_amount)) # add waypoints to dip the swab into the plate goal = (0.7, 0.35, 0.9) t = list(np.array(goal) - np.array([0.16, 0, 0])) objective = ik.objective(self.robot.link(9), R=so3.from_rpy( (-np.pi / 2, np.pi / 2, -np.pi / 2)), t=t) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('cannot place swab into plate') return None self.qplaceplate = self.robot.getConfig() self.qplaceplate = self.gripper.set_finger_config( self.qplaceplate, self.gripper.partway_open_config(self.close_amount)) t1 = list(np.array(t) - np.array([0, 0, 0.06])) objective = ik.objective(self.robot.link(9), R=so3.from_rpy( (-np.pi / 2, np.pi / 2, -np.pi / 2)), t=t1) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('cannot place swab into plate') return None self.qplaceplate_lower = self.robot.getConfig() self.qplaceplate_lower = self.gripper.set_finger_config( self.qplaceplate_lower, self.gripper.partway_open_config(self.close_amount)) # add waypoints to throw the swab into the trash can goal = (0.1, -0.4, 0.8) t = list(np.array(goal) - np.array([0.16, 0, 0])) objective = ik.objective(self.robot.link(9), R=so3.from_rpy( (-np.pi / 2, np.pi / 2, -np.pi / 2)), t=t) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: print('cannot place swab into plate') return None self.qplace_trash = self.robot.getConfig() self.qplace_trash = self.gripper.set_finger_config( self.qplace_trash, self.gripper.partway_open_config(self.close_amount)) self.qplace = self.gripper.set_finger_config( self.qplace_trash, self.gripper.partway_open_config(1)) else: Tplacement_x = se3.mul(Tplacement, se3.inv(self.Tobject_gripper)) objective = ik.objective(self.robot.link(9), R=Tplacement_x[0], t=Tplacement_x[1]) solved = ik.solve_global(objectives=objective, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7], feasibilityCheck=self.feasible) if not solved: return None qpreplace = self.robot.getConfig() qplace = self.gripper.set_finger_config( qpreplace, self.gripper.partway_open_config(1)) return qplace
def keyboardfunc(self, c, x, y): if c == 'h': print('HELP:') print('[right-click]: add a new IK constraint') print('[space]: tests the current configuration') print('d: deletes IK constraint') print('t: adds a new rotation-fixed IK constraint') print('f: flushes the current database to disk') print('s: saves the current database to disk') print('b: performs one background step') print('B: starts / stops the background thread') print('v: toggles display of the database') print( 'c: toggles continuous re-solving of IK constraint its as being moved' ) print('o: toggles soft / hard IK constraints') elif c == ' ': self.planningWorld.robot(0).setConfig(self.currentConfig) soln = self.ikdb.solve(self.ikProblem) if soln: print("Solved") self.currentConfig = soln self.refresh() else: print("Failure") elif c == 'd': for i, w in enumerate(self.ikWidgets): if w.hasHighlight(): print("Deleting IK widget") #delete it index = self.ikIndices[i] self.widgetMaster.remove(w) del self.ikWidgets[i] del self.ikIndices[i] del self.ikProblem.objectives[index] for j in range(len(self.ikIndices)): self.ikIndices[j] = j self.refresh() break elif c == 't': clicked = self.click_world(x, y) if clicked is not None and isinstance(clicked[0], RobotModelLink): #make a new widget link, wpt = clicked Tlink = link.getTransform() self.ikIndices.append(len(self.ikWidgets)) self.ikWidgets.append(TransformPoser()) self.ikWidgets[-1].set(*Tlink) self.widgetMaster.add(self.ikWidgets[-1]) self.ikProblem.addObjective( ik.objective(link, R=Tlink[0], t=Tlink[1])) self.refresh() elif c == 'f': self.ikdb.flush() elif c == 's': self.ikdb.save() elif c == 'b': self.ikdb.backgroundStep() self.refresh() elif c == 'B': if hasattr(self.ikdb, 'thread'): self.ikdb.stopBackgroundLoop() else: self.ikdb.startBackgroundLoop(0) elif c == 'v': self.drawDb = not self.drawDb elif c == 'c': self.continuous = not self.continuous elif c == 'o': self.ikProblem.setSoftObjectives(not self.ikProblem.softObjectives)
def control_loop(self): #Calculate the desired velocity for each robot by adding up all #commands rvels = [[0] * self.world.robot(r).numLinks() for r in range(self.world.numRobots())] robot = self.world.robot(0) ee_link = 6 ee_local_p1 = [-0.015, -0.02, 0.27] q_curr = robot.getConfig() q_curr[3] = -q_curr[3] q_curr[5] = -q_curr[5] q_output = vectorops.mul(q_curr[1:], angle_to_degree) milestone = (0.1, { 'robot': q_output, 'gripper': [0, 0, 0], 'vaccum': [0] }) self.output.append(milestone) if self.start_flag == 0: curr_position = robot.link(ee_link).getWorldPosition(ee_local) curr_orientation, p = robot.link(ee_link).getTransform() self.start_T = [curr_orientation, curr_position] self.start_flag = 1 local_p = [] if self.state == 'idle': t = 0.5 if self.score == self.numberOfObjects: print 'finished!' f = open('test.json', 'w') json.dump(self.output, f) f.close() exit() else: if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t self.end_T = [[0, 0, -1, 0, 1, 0, 1, 0, 0], idle_position] # self.idle_T=self.end_T # self.sim.controller(0).setPIDCommand(self.idleq,[0]*7) [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) # constraints=ik.objective(robot.link(ee_link),local=local_p,world=world_p) # traj1 = cartesian_trajectory.cartesian_interpolate_linear(robot,self.start_T,self.end_T,constraints,delta=1e-2,maximize=False) # print 'traj1:',traj1[0] else: # print 'idle', robot.getConfig() self.set_state('find_target') elif self.state == 'find_target': h = 0 for i in self.waiting_list: if self.sim.world.rigidObject(i).getTransform()[1][2] > h: self.target = i h = self.sim.world.rigidObject(i).getTransform()[1][2] print 'target is ', self.target # self.target=0 # self.object_p=self.sim.world.rigidObject(self.target).getTransform()[1] bb1, bb2 = self.sim.world.rigidObject( self.target).geometry().getBB() self.object_p = vectorops.div(vectorops.add(bb1, bb2), 2) # print 'object xform', self.sim.world.rigidObject(self.target).getTransform()[1] # print 'object_p',self.object_p h = self.object_p[2] + box_vacuum_offset[2] if self.object_p[1] > shelf_position[1]: end_p = approach_p1 end_p[2] = h else: end_p = approach_p2 end_p[2] = h d = vectorops.distance(end_p[:1], self.object_p[:1]) dx = self.object_p[0] - end_p[0] dy = self.object_p[1] - end_p[1] self.end_T = [[0, 0, -1, -dy / d, dx / d, 0, dx / d, dy / d, 0], end_p] print 'approach start position', end_p self.retract_T = [[ 0, 0, -1, -dy / d, dx / d, 0, dx / d, dy / d, 0 ], end_p] print 'retract_T', self.retract_T self.set_state('preposition') elif self.state == 'preposition': t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.end_T[1] = vectorops.add(self.object_p, [0.10, 0, 0.12]) self.set_state('pregrasp') elif self.state == 'pregrasp': t = 0.5 # print 'pregrasp' if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.end_T[1][2] -= 0.03 self.set_state('grasp') elif self.state == 'grasp': # print 'grasp' t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.end_T[1][2] += 0.03 self.set_state('prograsp') elif self.state == 'prograsp': # print 'grasp' self.graspedObjects[self.target] = True t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.end_T = self.retract_T self.end_T[1][2] += 0.01 self.set_state('retract') elif self.state == 'retract': # print 'retract' t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 1) else: self.set_state('go_to_tote') elif self.state == 'go_to_tote': x = robot.link(ee_link).getTransform()[1][0] if x > (order_box_min[0] + order_box_max[0]) / 2: q = robot.getConfig() q[1] += 0.02 # self.sim.controller(0).setPIDCommand(q,[0]*7) robotController.setLinear(q, 0.001) # self.output.append(vectorops.mul(q[1:],angle_to_degree)) else: bb1, bb2 = self.sim.world.rigidObject( self.target).geometry().getBB() bbx = bb2[0] - bb1[0] bby = bb2[1] - bb1[1] print 'BB:', bbx, bby # self.end_T[1]=[order_box_min[0]+bbx/2-0.01,order_box_min[1]+bby/2+0.21-self.target*0.1,0.8] self.end_T[1] = self.find_placement() # if self.score>0: # pass self.end_T[0] = [0, 0, -1, -1, 0, 0, 0, 1, 0] self.set_state('place') elif self.state == 'place': t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 0) else: self.end_T[1][2] -= 0.01 self.set_state('release') elif self.state == 'release': t = 0.5 if self.sim.getTime() - self.last_state_end < t: u = (self.sim.getTime() - self.last_state_end) / t [local_p, world_p] = interpolate(self.start_T, self.end_T, u, 0) else: self.graspedObjects[self.target] = False self.check_target() self.set_state('idle') old_T = robot.link(ee_link).getTransform() old_R, old_t = old_T if local_p: q_old = robot.getConfig() goal = ik.objective(robot.link(ee_link), local=local_p, world=world_p) # s=ik.solver(goal) # s.setMaxIters(100) # s.setTolerance(0.2) # if s.solve(): # pass # else: # print "IK failed" # while not s.solve(): # q_restart=[0,0,0,0,0,0,0] # q=robot.getConfig() # (qmin,qmax) = robot.getJointLimits() # for i in range(7): # q_restart[i]=random.uniform(-1,1)+q[i] # q_restart[i] = min(qmax[i],max(q_restart[i],qmin[i])) # print q_restart # robot.setConfig(q_restart) # if s.solve(): # print "IK solved" # else: # print "IK failed" # s=ik.solve_global(goal) s = ik.solve_nearby(goal, maxDeviation=10, feasibilityCheck=test_function) q = robot.getConfig() if not feasible(robot, q, q_old): s = ik.solve_global(goal) q = robot.getConfig() robotController = self.sim.controller(0) qdes = q (qmin, qmax) = robot.getJointLimits() for i in xrange(len(qdes)): qdes[i] = min(qmax[i], max(qdes[i], qmin[i])) # robotController.setPIDCommand(qdes,rvels[0]) robotController.setLinear(qdes, 0.001) # self.output.append(vectorops.mul(qdes[1:],angle_to_degree)) obj = self.sim.world.rigidObject(self.target) #get a SimBody object body = self.sim.body(obj) if self.graspedObjects[self.target]: T = body.getTransform() R, t = T body.enableDynamics(False) if self.flag == 0: self.flag = 1 self.t = robot.link(ee_link).getLocalPosition(t) # print 'here' new_T = robot.link(ee_link).getTransform() new_R, new_t = new_T change_R = so3.mul(new_R, so3.inv(old_R)) R = so3.mul(change_R, R) body.setTransform(R, robot.link(ee_link).getWorldPosition(self.t)) else: body.enableDynamics(True) self.flag = 0 return
if not MANUAL_SPACE_CREATION: space = robotplanning.makeSpace(world=world, robot=robot, edgeCheckResolution=1e-3, movingSubset='all') else: #Manual construction of space collider = WorldCollider(world) space = robotcspace.RobotCSpace(robot, collider) space.eps = 1e-3 space.setup() else: #TESTING: closed loop robot cspace collider = WorldCollider(world) obj = ik.objective(robot.link(robot.numLinks() - 1), local=[0, 0, 0], world=[0.5, 0, 0.5]) vis.add("IK goal", obj) vis.dialog() space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider) space.eps = 1e-3 space.setup() #Generate some waypoint configurations using the resource editor print("#########################################") print("Editing the waypoints") print("#########################################") configs = resource.get("planningtest.configs", "Configs", default=[], world=world,
def getTorque(t,q,dq): """ TODO: implement me Returns a 4-element torque vector given the current time t, the configuration q, and the joint velocities dq to drive the robot so that it traces out the desired curves. Recommended order of operations: 1. Monitor and update the current status and stroke 2. Update the stroke_progress, making sure to perform smooth interpolating trajectories 3. Determine a desired configuration given current state using IK 4. Send qdes, dqdes to PID controller """ global robot,status,current_stroke,current_stroke_progress,stroke_list, pid_integrators, interp, prevTarget, interpIncrement, t_up qdes = [0,0,0,0] dqdes = [0,0,0,0] link = robot.link(3) penDrawHeight = -0.001 if (status == "pen up") : penHeight = 0.005 elif (status == "pen up moving") : penHeight = 0.005 elif (status == "pen down") : #penHeight = -0.002 penHeight = penDrawHeight elif (status == "done") : penHeight = 0.005 else : #penHeight = -0.002 penHeight = penDrawHeight if (current_stroke == len(stroke_list)): status == "done" penHeight = 0.2 target = prevTarget else : target = stroke_list[current_stroke][current_stroke_progress] if (status == "move") : if interp > 0.9: delta = 0.08 interp = 1 + delta r1 = prevTarget[0] + interp * (target[0] - prevTarget[0]) r2 = prevTarget[1] + interp * (target[1] - prevTarget[1]) target = (r1,r2) wc = [] wc2 = [] wc.extend(target) wc.extend([penHeight]) wc2.extend(target) wc2.extend([penHeight+1]) # IK Solver obj = ik.objective(link,local= [[0,0,0],[0,0,-1]], world=[wc, wc2]) res = ik.solve(obj) qdes = robot.getConfig() kP = [20,8,1,20] kI = [0.05,0.01,0,0] kD = [8,1.5,0.1,5] allWithin = True for i in range(len(q)): acceptableRange = 0.002 if (status == "move" and interp <0.75) : acceptableRange = 0.01 if (status == "move" and interp >0.75) : acceptableRange = 0.001 if (math.fabs(q[i] - qdes[i]) > acceptableRange): allWithin = False if allWithin: if (status == "done") : status = "done" elif (status == "pen up") : interp = 0 status = "pen up moving" current_stroke_progress = 0; current_stroke = current_stroke + 1 elif (status == "pen up moving") : status = "pen down" elif (status == "pen down") : interp = interpIncrement status = "move" prevTarget = target current_stroke_progress = current_stroke_progress + 1 elif (status == "move"): if interp < (1 -.05): interp = interp + interpIncrement else: pid_integrators = [0,0,0,0] if (current_stroke_progress == len(stroke_list[current_stroke]) -1): t_up = t status = "pen up" if (current_stroke == len(stroke_list)-1) : status = "done" else : interp = interpIncrement prevTarget = target current_stroke_progress = current_stroke_progress+1 return getPIDTorqueAndAdvance(q,dq,qdes,dqdes,kP,kI,kD,pid_integrators,dt)
def getTorque(t,q,dq): """ TODO: implement me Returns a 4-element torque vector given the current time t, the configuration q, and the joint velocities dq to drive the robot so that it traces out the desired curves. Recommended order of operations: 1. Monitor and update the current status and stroke 2. Update the stroke_progress, making sure to perform smooth interpolating trajectories 3. Determine a desired configuration given current state using IK 4. Send qdes, dqdes to PID controller """ global robot,status,current_stroke,current_stroke_progress,stroke_list, pid_integrators, interp, prevTarget, interpIncrement qdes = [0,0,0,0] dqdes = [0,0,0,0] link = robot.link(3) if (status == "pen up") : print "PEN UP" penHeight = 0.05 elif (status == "pen up moving") : print "PEN UP MOVING" penHeight = 0.05 elif (status == "pen down") : print "PEN DOWN" penHeight = -0.002 else : print status penHeight = -0.002 if (current_stroke == len(stroke_list)): status == "done" penHeight = 0.2 target = (0, 0) else : target = stroke_list[current_stroke][current_stroke_progress] if (status == "move") : print "want to interp: ", interp r1 = prevTarget[0] + interp * (target[0] - prevTarget[0]) r2 = prevTarget[1] + interp * (target[1] - prevTarget[1]) target = (r1,r2) print target wc = [] wc2 = [] wc.extend(target) wc.extend([penHeight]) wc2.extend(target) wc2.extend([penHeight+1]) obj = ik.objective(link,local= [[0,0,0],[0,0,-1]], world=[wc, wc2]) res = ik.solve(obj) qdes = robot.getConfig() kP = [10,5,2,50] kI = [0,0,0,0] kD = [8,1.5,0.1,5] allWithin = True for i in range(len(q)): acceptableRange = 0.01 if (status == "moving" and interp <0.8) : acceptableRange = 0.05 if (status == "moving" and interp >0.8) : acceptableRange = 0.001 if (math.fabs(q[i] - qdes[i]) > acceptableRange): allWithin = False if allWithin: print "ALL WITHIN" if (status == "done") : print "do nothing" elif (status == "pen up") : interp = 0 status = "pen up moving" current_stroke_progress = 0; current_stroke = current_stroke + 1 elif (status == "pen up moving") : status = "pen down" elif (status == "pen down") : interp = interpIncrement status = "move" prevTarget = target print "prevTarget: ", prevTarget current_stroke_progress = current_stroke_progress + 1 elif (status == "move"): if interp < 1 - interpIncrement + interpIncrement/2: interp = interp + interpIncrement else: pid_integrators = [0,0,0,0] if (current_stroke_progress == len(stroke_list[current_stroke]) -1): print "END OF LIST" print current_stroke_progress status = "pen up" else : print "KEEP GOING" interp = interpIncrement prevTarget = target print "prevTarget: ", prevTarget print current_stroke_progress current_stroke_progress = current_stroke_progress+1 return getPIDTorqueAndAdvance(q,dq,qdes,dqdes,kP,kI,kD,pid_integrators,dt)
def keyboardfunc(self,c,x,y): if c=='h': print 'HELP:' print '[right-click]: add a new IK constraint' print '[space]: tests the current configuration' print 'd: deletes IK constraint' print 't: adds a new rotation-fixed IK constraint' print 'f: flushes the current database to disk' print 's: saves the current database to disk' print 'b: performs one background step' print 'B: starts / stops the background thread' print 'v: toggles display of the database' print 'c: toggles continuous re-solving of IK constraint its as being moved' print 'o: toggles soft / hard IK constraints' elif c==' ': self.planningWorld.robot(0).setConfig(self.currentConfig) soln = self.ikdb.solve(self.ikProblem) if soln: print "Solved" self.currentConfig = soln self.refresh() else: print "Failure" elif c=='d': for i,w in enumerate(self.ikWidgets): if w.hasHighlight(): print "Deleting IK widget" #delete it index = self.ikIndices[i] self.widgetMaster.remove(w) del self.ikWidgets[i] del self.ikIndices[i] del self.ikProblem.objectives[index] for j in range(len(self.ikIndices)): self.ikIndices[j] = j self.refresh() break elif c=='t': clicked = self.click_world(x,y) if clicked is not None and isinstance(clicked[0],RobotModelLink): #make a new widget link, wpt = clicked Tlink = link.getTransform() self.ikIndices.append(len(self.ikWidgets)) self.ikWidgets.append(TransformPoser()) self.ikWidgets[-1].set(*Tlink) self.widgetMaster.add(self.ikWidgets[-1]) self.ikProblem.addObjective(ik.objective(link,R=Tlink[0],t=Tlink[1])) self.refresh() elif c=='f': self.ikdb.flush() elif c=='s': self.ikdb.save() elif c=='b': self.ikdb.backgroundStep() self.refresh() elif c=='B': if hasattr(self.ikdb,'thread'): self.ikdb.stopBackgroundLoop() else: self.ikdb.startBackgroundLoop(0) elif c=='v': self.drawDb = not self.drawDb elif c=='c': self.continuous = not self.continuous elif c == 'o': self.ikProblem.setSoftObjectives(not self.ikProblem.softObjectives)
def modification_template(world): """Tests a variety of miscellaneous vis functions""" vis.add("world",world) robot = world.robot(0) vis.setColor(("world",world.terrain(0).getName()),1,0,0,0.5) #turn the terrain red and 50% opaque import random for i in range(10): #set some links to random colors randlink = random.randint(0,robot.numLinks()-1) color = (random.random(),random.random(),random.random()) vis.setColor(("world",robot.getName(),robot.link(randlink).getName()),*color) #test the on-screen text display vis.addText("text2","Here's some red text") vis.setColor("text2",1,0,0) vis.addText("text3","Here's bigger text") vis.setAttribute("text3","size",24) vis.addText("text4","Transform status") vis.addText("textbottom","Text anchored to bottom of screen",(20,-30)) #test a point pt = [2,5,1] vis.add("some point",pt) #test a rigid transform vis.add("some blinking transform",[so3.identity(),[1,3,0.5]]) vis.edit("some point") #vis.edit("some blinking transform") #vis.edit("coordinates:ATHLETE:ankle roll 3") #test an IKObjective link = world.robot(0).link(world.robot(0).numLinks()-1) #point constraint obj = ik.objective(link,local=[[0,0,0]],world=[pt]) #hinge constraint #obj = ik.objective(link,local=[[0,0,0],[0,0,0.1]],world=[pt,[pt[0],pt[1],pt[2]+0.1]]) #transform constraint #obj = ik.objective(link,R=link.getTransform()[0],t=pt) vis.add("ik objective",obj) #enable plotting vis.addPlot('plot') vis.addPlotItem('plot','some point') vis.setPlotDuration('plot',10.0) #run the visualizer, which runs in a separate thread vis.setWindowTitle("Manual animation visualization test") class MyCallback: def __init__(self): self.iteration = 0 def __call__(self): vis.lock() #TODO: you may modify the world here. This line tests a sin wave. pt[2] = 1 + math.sin(self.iteration*0.03) vis.unlock() #changes to the visualization with vis.X functions can done outside the lock if (self.iteration % 100) == 0: if (self.iteration / 100)%2 == 0: vis.hide("some blinking transform") vis.addText("text4","The transform was hidden") vis.logPlotEvent('plot','hide') else: vis.hide("some blinking transform",False) vis.addText("text4","The transform was shown") vis.logPlotEvent('plot','show') #this is another way of changing the point's data without needing a lock/unlock #vis.add("some point",[2,5,1 + math.sin(iteration*0.03)],keepAppearance=True) #or #vis.setItemConfig("some point",[2,5,1 + math.sin(iteration*0.03)]) if self.iteration == 200: vis.addText("text2","Going to hide the text for a second...") if self.iteration == 400: #use this to remove text vis.clearText() if self.iteration == 500: vis.addText("text2","Text added back again") vis.setColor("text2",1,0,0) self.iteration += 1 callback = MyCallback() if not MULTITHREADED: vis.loop(callback=callback,setup=vis.show) else: vis.show() while vis.shown(): callback() time.sleep(0.01) #use this to remove a plot vis.remove("plot") vis.kill()
def add_milestones(test_cspace, robot, milestones, t, control_rate, start_T, end_T, vacuum_status, simulation_status, ik_indicator): if t < 0.3: t = 0.3 steps = t * control_rate t_step = 1.0 / control_rate # print "start config",robot.getConfig() i = 0 start_q = robot.getConfig()[3] while i <= steps: q_old = robot.getConfig() u = i * 1.0 / steps # print u [local_p, world_p] = interpolate(start_T, end_T, u, ik_indicator) goal = ik.objective(robot.link(ee_link), local=local_p, world=world_p) s = ik.solve_global(goal) # s=ik.solve_nearby(goal,maxDeviation=1000,feasibilityCheck=test_function) q = robot.getConfig() n = 0 # print i # print s # print test_cspace.feasible(q) flag = 1 if (max(vectorops.sub(q_old, q)) > max_change) or (min(vectorops.sub(q_old, q)) < (-max_change)) or q[3] * start_q < 0: print "too much change!" print vectorops.sub(q_old, q) flag = 0 while n < 30: if flag and (s and test_cspace.feasible(q)): break else: # print "no feasible ik solution found" # print world_p robot.setConfig(q_old) s = ik.solve_global(goal) # s=ik.solve_nearby(goal,maxDeviation=1000,feasibilityCheck=test_function) q = robot.getConfig() if (max(vectorops.sub(q_old, q)) > max_change) or (min(vectorops.sub(q_old, q)) < (-max_change)) or q[3] * start_q < 0: print "too much change!" flag = 0 else: flag = 1 # print 's',s # print 'feasible test:',test_cspace.feasible(q) n += 1 if flag and s and test_cspace.feasible(q): m_change = max(max(vectorops.sub(q_old, q)), -min(vectorops.sub(q_old, q))) milestones.append( make_milestone(t_step, q, vacuum_status, simulation_status)) i += 1 else: print 'no feasible solution can be found!!!!!' print s return False return milestones