def generateGoal(self): self.goal = TrajectoryGoal() self.goal.times = self.interp_times self.goal.waypoints = [] for i in range(0, self.num_interp_waypoints): waypoint = WaypointMsg() waypoint.names = self.names waypoint.velocities = [NaN, NaN, NaN, NaN] waypoint.accelerations = [NaN, NaN, NaN, NaN] workspace_waypoint = [] # construct workspace waypoint for j in range(0, self.num_workspace_dof): workspace_waypoint.append( \ self.workspace_waypoints[j][i]) # get elbow up elbow = self.elbow_up[0] for j in range(0, self.num_waypoints): if (self.interp_times[i] <= self.times[j]): elbow = self.elbow_up[j] # run ik to get configspace waypoint configuration_waypoint = kin.ik(workspace_waypoint, elbow) waypoint.positions = configuration_waypoint self.goal.waypoints.append(waypoint) self.goal.waypoints[0].velocities = [0, 0, 0, 0] self.goal.waypoints[0].accelerations = [0, 0, 0, 0] self.goal.waypoints[-1].velocities = [0, 0, 0, 0] self.goal.waypoints[-1].accelerations = [0, 0, 0, 0]
def static_position(self, position=[0, 0, 0]): angles = [] for j in range(6): leg = tip_to_leg(j, position) angle = ik(leg) angles.extend(angle) # print(j, leg, angle) self.cbServoAngles(angles) time.sleep(0.05)
def run_ik(q_des): """ Moves the robot Args: q_des - (3,) ndarray: desired position Returns: """ g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES try: joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position theta = kin.ik(q_des) if theta[0] > 0: if abs(joints_pos[0] - theta[0]) > abs(joints_pos[0] - (theta[0] - math.pi * 2)): theta[0] = theta[0] - 2 * math.pi elif theta[0] < 0: if abs(joints_pos[0] - theta[0]) > abs(joints_pos[0] - (theta[0] + math.pi * 2)): theta[0] = theta[0] + 2 * math.pi theta[3] = -theta[1] - theta[2] - math.pi / 2 # print kin.rad2deg(theta) # inp = raw_input("Continue? y/n: ")[0] inp = 'y' if (inp == 'y'): g.trajectory.points = [ JointTrajectoryPoint(positions=joints_pos, velocities=[0] * 6, time_from_start=rospy.Duration(0.0)), JointTrajectoryPoint(positions=theta, velocities=[0] * 6, time_from_start=rospy.Duration(10.0)) ] client.send_goal(g) client.wait_for_result() else: print "Halting program" except KeyboardInterrupt: client.cancel_goal() raise except: raise
def verify_points(pt): angles = kinematics.ik(pt) ok = True failed = [] for i, angle in enumerate(angles): if angle < config.angleLimitation[i][ 0] or angle > config.angleLimitation[i][1]: ok = False failed.append((i, angle)) return ok, failed
def static_rotate(self, rotate=[0, 0, 0]): angles = [] position = [0, 0, 0] position = config.defaultPosition for j in range(6): pt = position[j] pt = point_rotate(pt, rotate) leg = global_to_leg(j, pt) angle = ik(leg) angles.extend(angle) # print(j, leg, angle) self.cbServoAngles(angles)
def translate_with_rotate(self, translate=[0, 0, 0], rotate=[0, 0, 0]): angles = [] # position = [0, 0, 0] position = config.defaultPosition for j in range(6): # pt = position[j] pt = point_add(position[j], translate) pt = point_rotate(pt, rotate) leg = global_to_leg(j, pt) angle = ik(leg) angles.extend(angle) # print(j, leg, angle) self.cbServoAngles(angles)
def test_ik(): global joints_pos g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES q_des = np.array([-.8, -.23, .018]) try: joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position listener = tf.TransformListener() a = 0 while a == 0: try: (trans, rot) = listener.lookupTransform('base', 'ee_link', rospy.Time(0)) a = 1 except Exception as e: a = 0 theta = kin.ik(q_des) print kin.rad2deg(theta) raw_input() g.trajectory.points = [ JointTrajectoryPoint(positions=joints_pos, velocities=[0] * 6, time_from_start=rospy.Duration(0.0)), JointTrajectoryPoint(positions=theta, velocities=[0] * 6, time_from_start=rospy.Duration(10.0)) ] client.send_goal(g) client.wait_for_result() except KeyboardInterrupt: client.cancel_goal() raise except: raise
def computePoints(to): y, x, z = to angles = ik(to) rads = angles/180.0*np.pi po = [0, 0, 0] pb = [0, 0, -z] bh = point_rotate_y([config.ABDUCTION_OFFSET, 0, 0], angles[0]) bk = point_rotate_y([config.ABDUCTION_OFFSET, -config.LEG_L1 * math.sin(rads[1]), -config.LEG_L1 * math.cos(rads[1])], angles[0]) bm = point_rotate_y([config.ABDUCTION_OFFSET, 0, - config.LEG_L1 * math.cos(rads[1])], angles[0]) bn = point_rotate_y([config.ABDUCTION_OFFSET, -config.LEG_L1 * math.sin(rads[1]), -(config.LEG_L1 * math.cos(rads[1])+config.LEG_L2 * math.cos(rads[2]))], angles[0]) pm = np.array(bm)+np.array(pb) pn = np.array(bn)+np.array(pb) ph = [bh[0], bh[1], bh[2]-z] pk = [bk[0], bk[1], bk[2]-z] pf = [x, -y, 0] points = [po, pb, ph, pk, pf] Y = [x, 0, 0] X = [0, -y, 0] assists = [pm, pn, Y] return points, assists
def move_sub(client, joint_states, JOINT_NAMES, q_des, speed): """ Actually moves the arm """ q_current = KIN.fk(joint_states) q_ik = q_des # print "Goal: ", q_des while True: final_states = KIN.ik(q_ik) ## Hacky fixes # Fix theta4 final_states[3] = -final_states[2] - final_states[1] - math.pi / 2 # Stop theta1 from going all the way around if final_states[0] > 0: if abs(joint_states[0] - final_states[0]) > abs(joint_states[0] - (final_states[0] - math.pi * 2)): final_states[0] = final_states[0] - 2 * math.pi elif final_states[0] < 0: # print "yo" # print abs(joint_states[0] - final_states[0]) # print abs(joint_states[0] - (final_states[0] + math.pi*2)) if abs(joint_states[0] - final_states[0]) > abs(joint_states[0] - (final_states[0] + math.pi * 2)): final_states[0] = final_states[0] + 2 * math.pi q_current = KIN.fk(final_states) # print "Current p:", q_current, "Distance: ", np.linalg.norm(q_current - q_des) if np.linalg.norm(q_current - q_des) <= 0.001: break else: q_ik = q_ik + (q_des - q_current) / 2 # raw_input("Check before moving") # print final_states # raw_input() # Actually move move_speed = speed g = FollowJointTrajectoryGoal() g.trajectory = JointTrajectory() g.trajectory.joint_names = JOINT_NAMES try: g.trajectory.points = [ JointTrajectoryPoint(positions=joint_states, velocities=[0] * 6, time_from_start=rospy.Duration(0.0)), JointTrajectoryPoint(positions=final_states, velocities=[0] * 6, time_from_start=rospy.Duration(move_speed)) ] client.send_goal(g) client.wait_for_result() except KeyboardInterrupt: client.cancel_goal() raise except: raise
path_rotate_z(mir_path, 135), ] def turn_right_generator(direction=180): assert (g_steps % 4) == 0 halfsteps = int(g_steps/2) path = any_direction_generator( g_radius, g_steps, direction=direction, reverse=False) mir_path = deque(path) mir_path.rotate(halfsteps) return [path_rotate_z(path, 45), path_rotate_z(mir_path, 0), path_rotate_z(path, 315), path_rotate_z(mir_path, 225), path_rotate_z(path, 180), path_rotate_z(mir_path, 135), ] if __name__ == '__main__': from kinematics import tip_to_leg, ik, leg_ki path = path_generator() seq = path[0] # seq = [[0, 0, 0]] for point in seq: leg = tip_to_leg(0, point) angle = ik(leg) print('leg', leg) print('angle', angle) print('position', leg_ki(0, angle))
def getJointStateCommand(self, time): cmd = JointState() cmd.name = self.names # find previous which leg we are on if (time < 0 or time > self.times[-1]): return null index = -1 for i in range(0, len(self.times) - 1): if (self.times[i] <= time and time <= self.times[i + 1]): index = i break if (index == -1): return null cmd.position = list([0, 0, 0, 0]) cmd.velocity = list([0, 0, 0, 0]) cmd.effort = list([NaN, NaN, NaN, NaN]) epsilon = 0.001 leg_duration = self.times[index + 1] - self.times[index] leg_percentage = 1.0 - ((self.times[index + 1] - time) / leg_duration) leg_percentage_epsilon = 1.0 - ((self.times[index + 1] - (time + epsilon)) / leg_duration) print leg_percentage print leg_percentage_epsilon # Calculate current workspace waypoint wayp = list([0, 0, 0, 0, 0]) wayp_epsilon = list([0, 0, 0, 0, 0]) prev_wayp = \ [self.waypoints[0][index], self.waypoints[1][index], self.waypoints[2][index], self.waypoints[3][index], self.waypoints[4][index]] next_wayp = \ [self.waypoints[0][index+1], self.waypoints[1][index+1], self.waypoints[2][index+1], self.waypoints[3][index+1], self.waypoints[4][index+1]] for dof in range(0, 5): difference = next_wayp[dof] - prev_wayp[dof] wayp[dof] = prev_wayp[dof] + (leg_percentage * difference) wayp_epsilon[dof] = prev_wayp[dof] + (leg_percentage_epsilon * difference) print wayp print wayp_epsilon cmd.position = kin.ik(wayp, self.elbow_up[index]) position_epsilon = kin.ik(wayp_epsilon, self.elbow_up[index]) for joint in range(0, 4): cmd.velocity[joint] = (cmd.position[joint] - position_epsilon[joint]) / epsilon # Calculate current joint velocity) # Calculate workspace waypoint a small time in the future wayp_epsilon = list([0, 0, 0, 0, 0]) for dof in range(0, 5): difference = next_wayp[dof] - prev_wayp[dof] wayp[dof] = prev_wayp[dof] + (leg_percentage * leg_duration) return cmd
cmd.position = kin.ik(wayp, self.elbow_up[index]) position_epsilon = kin.ik(wayp_epsilon, self.elbow_up[index]) for joint in range(0, 4): cmd.velocity[joint] = (cmd.position[joint] - position_epsilon[joint]) / epsilon # Calculate current joint velocity) # Calculate workspace waypoint a small time in the future wayp_epsilon = list([0, 0, 0, 0, 0]) for dof in range(0, 5): difference = next_wayp[dof] - prev_wayp[dof] wayp[dof] = prev_wayp[dof] + (leg_percentage * leg_duration) return cmd if __name__ == '__main__': t = TrajectoryGenerator(["a", "b", "c", "d"]) t.set_initial_pose(kin.ik([0.2, 0, 0, 0, 0], True)) t.addWaypoint([0.6, 0, 0, 0, 0], 1, True) time = 0 while (time <= t.getDuration()): cmd = t.getJointStateCommand(time) print kin.fk(cmd.position) print cmd.velocity print "\n" time = time + 0.1
print(x_offset, y_offset, z_offset) x_offset = x_offset + deltaMax_x_offset y_offset = y_offset + deltaMax_y_offset z_offset = z_offset + deltaMax_z_offset walkEngine.changeDir([x, y], splineTick) fl_x, fl_y, fl_z = walkEngine.getFLPos(splineTick) fr_x, fr_y, fr_z = walkEngine.getFRPos(splineTick) br_x, br_y, br_z = walkEngine.getBRPos(splineTick) bl_x, bl_y, bl_z = walkEngine.getBLPos(splineTick) t1_fl, t2_fl, t3_fl = rescale_angles( kinematics.ik(initX + fl_x + x_offset, initY + fl_y + y_offset, initZ + fl_z + z_offset, L1, L2, L3)) t1_bl, t2_bl, t3_bl = rescale_angles( kinematics.ik(initX + bl_x + x_offset, initY + bl_y + y_offset, initZ + bl_z + z_offset, L1, L2, L3)) t1_fr, t2_fr, t3_fr = rescale_angles( kinematics.ik((x_offset + fr_x) - initX, y_offset + fr_y + initY, z_offset + fr_z + initZ, -L1, L2, L3)) t1_br, t2_br, t3_br = rescale_angles( kinematics.ik((x_offset + br_x) - initX, y_offset + br_y + initY, z_offset + br_z + initZ, -L1, L2, L3)) targets["front_left_shoulder"] = t1_fl + np.pi / 2 targets["front_left_thigh"] = t2_fl + np.pi / 2 targets["front_left_leg"] = t3_fl + np.pi / 2 targets["back_left_shoulder"] = -(t1_bl + np.pi / 2)