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)
