def leg_ik(leg, claw, times={}): """ Takes: leg: leg data object claw: desired claw location in cylindrical coordinates in leg frame Returns: [h, e, k] list If inputs are invalid, returns a list of length 4 """ # Timing value tv_leg_ik = time() # Constants hyp = ((claw[1] - leg.trolen)**2 + claw[2]**2)**.5 try: # Inverse Kinematics h = claw[0] - leg.gamma e = helpers.rtod( m.asin((claw[1] - leg.trolen) / hyp) + m.acos((leg.tiblen**2 - leg.femlen**2 - hyp**2) / (-2 * leg.femlen * hyp)) - (m.pi / 2)) - 90 k = helpers.rtod(m.pi - m.acos((hyp**2 - leg.tiblen**2 - leg.femlen**2) / (-2 * leg.tiblen * leg.femlen))) - 180 times = helpers.dict_timer("Ik.leg_ik", times, time() - tv_leg_ik) return np.array([fix_angles_2(h), fix_angles_2(e), fix_angles_2(k)]) except Exception as e: times = helpers.dict_timer("Ik.leg_ik", times, time() - tv_leg_ik) print("Invalid parameters. Leg ik failed.") return np.array([0, 1, 2, 3]) # Arbitray list of 4 to indicate ik failure
def set_target_positions(pos_list): global times if (len(pos_list) != len(IDS)): print("pos_list doesn't have the same dimension as ids_list") return -1 for i in range(len(pos_list)): # write goal position dxl_addparam_result = dynamixel.groupSyncWriteAddParam( GROUP_NUM, IDS[i], pos_list[i] + HOME, LEN_MX_GOAL_POSITION) if dxl_addparam_result != 1: print(dxl_addparam_result) """dynamixel.write2ByteTxRx(PORT_NUM, PROTOCOL_VERSION, IDS[i], ADDR_MX_GOAL_POSITION, pos_list[i] + HOME) dxl_comm_result = dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(PORT_NUM, PROTOCOL_VERSION) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)) return -1 elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) return -1""" tv_gswtp = time() dynamixel.groupSyncWriteTxPacket(GROUP_NUM) times = helpers.dict_timer("DT.groupSyncWriteTxPacket", times, time() - tv_gswtp) # if dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION) != COMM_SUCCESS: # dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION)) tv_gswcp = time() dynamixel.groupSyncWriteClearParam(GROUP_NUM) times = helpers.dict_timer("DT.groupSyncWriteClearParam", times, time() - tv_gswcp)
def update_robot(body, current_state, dt): global ctr global t global was_still global times # Timing value tv_update_robot = time() # read state enable = bool(current_state["enable"]) vx = float(current_state["vx"]) vy = float(current_state["vy"]) omega = float(current_state["omega"]) height = float(current_state["height"]) pitch = float(current_state["pitch"]) roll = float(current_state["roll"]) yaw = float(current_state["yaw"]) pan = float(current_state["pan"]) tilt = float(current_state["tilt"]) home_wid = float(current_state["homewidth"]) home_len = float(current_state["homelength"]) # timestep = current_state[timestep] stridelength = float(current_state["stridelength"]) raisefrac = float(current_state["raisefrac"]) raiseh = float(current_state["raiseh"]) lift_phase = float(current_state["liftphase"]) phase0 = float(current_state["phasefl"]) phase1 = float(current_state["phasebl"]) phase2 = float(current_state["phasebr"]) phase3 = float(current_state["phasefr"]) return_home = bool(current_state["gohome"]) # call timestep function sleeptime, angles, t, was_still, times = gait_alg_test.timestep( body, enable, return_home, vx, vy, omega, height, pitch, roll, yaw, t, home_wid, home_len, dt, stridelength, raisefrac, raiseh, lift_phase, [phase0, phase1, phase2, phase3], was_still, times) angles.append(pan) angles.append(tilt) times = helpers.dict_timer("DT.update_robot", times, time() - tv_update_robot) # update servos accordingly: error check is that when given impossible values IK returns array of angles of incorrect length if (len(angles) == 14): # Timing print tv_stp = time() err = set_target_positions(check_angles(deg_to_dyn(angles))) times = helpers.dict_timer("DT.set_target_positions", times, time() - tv_update_robot) if (ctr > num_iters): ctr = 0 for k in times.keys(): print(k, "time: ", times[k] / num_iters) times[k] = 0 print("\n") ctr += 1
def body_ik(body, claws, pitch, roll, height, zs, times={}): # Timing value tv_body_ik = time() # copy claws to prevent error propagation and add z coordinate # hclaws = copy.copy(claws) # create rotation matrix pc, ps, rc, rs = m.cos(helpers.dtor(pitch)), m.sin(helpers.dtor(pitch)), m.cos(helpers.dtor(roll)), m.sin(helpers.dtor(roll)) pitchm = np.matrix([[pc, 0, -ps], [0, 1, 0], [ps, 0, pc]]) rollm = np.matrix([[1, 0, 0], [0, rc, -rs], [0, rs, rc]]) rot = pitchm * rollm # put leg offsets through rotation and subtract difference newclaws = [] for i in range(len(body.legs)): claws[i] = np.append(claws[i], -1 * height) vec = rot * body.legs[i].off[np.newaxis].T newclaws.append(helpers.tocyl(claws[i] - np.squeeze(np.asarray(vec)))) # incorporate leg lifts for i in range(len(zs)): if (zs[i] > 0): newclaws[i][2] = -1*(height-zs[i]) times = helpers.dict_timer("Ik.body_ik", times, time()-tv_body_ik) return newclaws
def update_leg(state, vx, vy, omega, t, lift_phase, timestep, stridelength, raisefrac, raiseh, times={}): """Updates the state of a leg at each timestep, given state and robot velocity""" # Timing value tv_ul = time() # The phase of the walking cycle is calculated phase = (t + state.phase_offset * stridelength) % stridelength # If the leg is being lifted if phase < lift_phase: state.home_offs[0] = -1 * (-vx + state.yawhomes[1] * omega) * ( stridelength * (1 - lift_phase) / 2) state.home_offs[1] = -1 * (-vy - state.yawhomes[0] * omega) * ( stridelength * (1 - lift_phase) / 2) # Move it horizontally towards the home position state.loc[0] += calculate_step(state.loc[0], state.yawhomes[0] + state.home_offs[0], phase, lift_phase, timestep, stridelength, times) state.loc[1] += calculate_step(state.loc[1], state.yawhomes[1] + state.home_offs[1], phase, lift_phase, timestep, stridelength, times) # If it's being lifted if phase < lift_phase * raisefrac: # Lift it state.loc[2] += calculate_step(state.loc[2], raiseh, phase, lift_phase * raisefrac, timestep, stridelength, times) # If it's being lowered, elif phase > lift_phase * (1 - raisefrac): # Lower it state.loc[2] += calculate_step(state.loc[2], 0, phase, lift_phase, timestep, stridelength, times) else: # Otherwise, move it horizontally based on robot velocity state.loc[0] += (-vx + state.loc[1] * omega) * timestep state.loc[1] += (-vy - state.loc[0] * omega) * timestep times = helpers.dict_timer("GAT.update_leg", times, time() - tv_ul)
def leg_ik(leg, claw, times={}): # Timing value tv_leg_ik = time() # Constants hyp = ((claw[1] - leg.trolen)**2 + claw[2]**2)**.5 try: # Inverse Kinematics h = claw[0] - leg.gamma e = helpers.rtod(m.asin((claw[1] - leg.trolen)/hyp) + m.acos((leg.tiblen**2 - leg.femlen**2 - hyp**2)/(-2 * leg.femlen * hyp)) - (m.pi/2)) - 90 k = helpers.rtod(m.pi - m.acos((hyp**2 - leg.tiblen**2 - leg.femlen**2)/(-2 * leg.tiblen * leg.femlen))) - 180 times = helpers.dict_timer("Ik.leg_ik", times, time()-tv_leg_ik) return np.array([fix_angles_2(h), fix_angles_2(e), fix_angles_2(k)]) except: times = helpers.dict_timer("Ik.leg_ik", times, time()-tv_leg_ik) print("Invalid parameters. Leg ik failed.") return np.array([0, 1, 2, 3])
def body_ik(body, claws, pitch, roll, height, zs, times={}): """ Takes: body: body.legs contains the list of leg data objects claws: list of desired claw locations in floor plane pitch: desired pitch of robot roll: desired roll of robot height: desired height of robot zs: (Don't really know) times: (Don't really know) Returns: newclaws: list of desired claw positions in cylindrical coordinates in leg frame of rotated robot """ # Timing value tv_body_ik = time() # copy claws to prevent error propagation and add z coordinate # hclaws = copy.copy(claws) # create rotation matrix pc, ps, rc, rs = m.cos(helpers.dtor(pitch)), m.sin( helpers.dtor(pitch)), m.cos(helpers.dtor(roll)), m.sin( helpers.dtor(roll)) pitchm = np.matrix([[pc, 0, -ps], [0, 1, 0], [ps, 0, pc]]) rollm = np.matrix([[1, 0, 0], [0, rc, -rs], [0, rs, rc]]) rot = pitchm * rollm # put leg offsets through rotation and subtract difference newclaws = [] for i in range(len(body.legs)): claws[i] = np.append(claws[i], -1 * height) vec = rot * body.legs[i].off[np.newaxis].T newclaws.append(helpers.tocyl(claws[i] - np.squeeze(np.asarray(vec)))) # incorporate leg lifts for i in range(len(zs)): if (zs[i] > 0): newclaws[i][2] = -1 * (height - zs[i]) times = helpers.dict_timer("Ik.body_ik", times, time() - tv_body_ik) return newclaws
def calculate_step(cur, goal, phase, endphase, timestep, stridelength, times={}): """Calculate the next step to reach a goal at a certain time""" # Timing value tv_cs = time() # Check if the end phase is very close or past if phase > endphase - macros.TOLERANCE: # If so, go to the goal right away return goal - cur times = helpers.dict_timer("GAT.calculate_step", times, time() - tv_cs) # Otherwise, go at the speed that will reach the goal at the target time return (goal - cur) * timestep / (stridelength * (endphase - phase))
def extract_angles(body, claws, pitch=macros.DEFAULT_PITCH, roll=macros.DEFAULT_ROLL, height=macros.DEFAULT_HEIGHT, zs=[0, 0, 0, 0], times={}): """ Takes: body: body_data object claws: list of claw positions pitch: desired pitch in degrees roll: desired roll in degrees height: desired height zs: (Don't really know) times: (Don't really know) Returns: list of angles [h1, e1, k1, h2, e2, k2, h3, e3, k3, h4, e4, k4] """ # Timing value tv_ea = time() # Returns np.array # newclaws = body_ik(body, claws, pitch, roll, height, zs, times) # ret_angles = np.array([]) # for i in range(len(body.legs)): # ret_angles = np.append(ret_angles, (leg_ik(body.legs[i], newclaws[i], times))) # Returns list newclaws = body_ik(body, claws, pitch, roll, height, zs, times) ret_angles = [] for i in range(len(body.legs)): toadd = leg_ik(body.legs[i], newclaws[i], times) for j in toadd: ret_angles.append(j) times = helpers.dict_timer("Ik.extract_angles", times, time() - tv_ea) return ret_angles
def extract_angles(body, claws, pitch=macros.DEFAULT_PITCH, roll=macros.DEFAULT_ROLL, height=macros.DEFAULT_HEIGHT, zs=[0,0,0,0], times={}): # Timing value tv_ea = time() # Returns np.array # newclaws = body_ik(body, claws, pitch, roll, height, zs, times) # ret_angles = np.array([]) # for i in range(len(body.legs)): # ret_angles = np.append(ret_angles, (leg_ik(body.legs[i], newclaws[i], times))) # Returns list newclaws = body_ik(body, claws, pitch, roll, height, zs, times) ret_angles = [] for i in range(len(body.legs)): toadd = leg_ik(body.legs[i], newclaws[i], times) for j in toadd: ret_angles.append(j) times = helpers.dict_timer("Ik.extract_angles", times, time()-tv_ea) return ret_angles
vx = 0 vy = 0 omega = 0 height = 8 pitch = 0 roll = 0 yaw = 20 * m.sin(t) home_wid = 9 home_len = 9 sleeptime, angles, t, was_still, times = gait_alg.timestep( body, vx, vy, omega, height, pitch, roll, yaw, t, home_wid, home_len, was_still, times) times = helpers.dict_timer("Cont.update_robot", times, time() - tv_update_robot) if (len(angles) == 12): tv_servosend = time() client.send(hlsockets.SERVO, quick_fix_angles(angles)) times = helpers.dict_timer("Cont.servosend", times, time() - tv_servosend) sleep(sleeptime) # if (ctr > num_iters): # ctr = 0 # for k in times.keys(): # print(k, "time: ", times[k]/num_iters) # times[k]=0 # print("\n")
def timestep(body, enable, return_home, vx, vy, omega, height, pitch, roll, yaw, t, home_wid, home_len, timestep, stridelength, raisefrac, raiseh, lift_phase, phases, was_still, times): """Updates the states of every leg for a given robot body, given state and robot velocity""" # Timing value tv_timestep = time() timestep = macros.TIMESTEP # Update leg states for i in range(len(body.legs)): body.legs[i].state.phase_offset = phases[i] body.legs[i].state.homes[0] = body.legs[i].state.signs[0] * home_len body.legs[i].state.homes[1] = body.legs[i].state.signs[1] * home_wid # Variables to track change in state xys = [] zs = [] # Manage yawing yc, ys = m.cos(helpers.dtor(yaw)), m.sin(helpers.dtor(yaw)) yawm = np.matrix([[yc, -ys], [ys, yc]]) for i in range(len(body.legs)): body.legs[i].state.yawhomes = np.squeeze( np.asarray(yawm * body.legs[i].state.homes[np.newaxis].T)) # yawc, yaws = m.cos(helpers.dtor(yaw)), m.sin(helpers.dtor(yaw)) # for i in range(len(body.legs)): # home_x, home_y = body.legs[i].state.homes[0], body.legs[i].state.homes[1] # body.legs[i].state.yawhomes = [home_x * yawc - home_y * yaws, home_x * yaws + home_y * yawc] # If walking is enabled and the robot is at the minimum required velocity if (enable and ((m.sqrt(vx**2 + vy**2) >= macros.MIN_V) or (abs(omega) >= macros.MIN_OMEGA))): # If robot was still immediately before this timestep if (was_still): t = 0 was_still = False # Update each leg for i in range(len(body.legs)): update_leg(body.legs[i].state, vx, vy, helpers.dtor(omega), t, lift_phase, timestep, stridelength, raisefrac, raiseh, times) xys.append([body.legs[i].state.loc[0], body.legs[i].state.loc[1]]) zs.append(body.legs[i].state.loc[2]) # Else the claws should be static else: was_still = True for i in range(len(body.legs)): xys.append(body.legs[i].state.yawhomes) zs.append(0) # Increment timestep t += timestep times = helpers.dict_timer("GAT.timestep", times, time() - tv_timestep) ret_angles = ik.extract_angles(body, xys, pitch, roll, height, zs, times) # Return formatted array of angles return (timestep, ret_angles, t, was_still, times)