def check(self, xyz_pos, printMessages=False):
if not self.isBetween(xyz_pos, self.xyz_min, self.xyz_max):
if printMessages: print("xyz out of range")
return False
iksolver = IKSolver()
theta = iksolver.solve(xyz_pos)
if len(theta) == 0 or not self.isBetween(theta, [self.mintheta] * 3,
[self.maxtheta] * 3):
if printMessages: print("no IK solution or theta out of range")
return False
xend = xyz_pos[0] - self.gripper_center_x_offset
yend = xyz_pos[1] - self.gripper_center_y_offset
zend = xyz_pos[2]
phi0 = math.asin(xend / self.l)
phi1 = math.asin((-0.5 * xend - math.sqrt(3) / 2 * yend) / self.l)
phi2 = math.asin((-0.5 * xend + math.sqrt(3) / 2 * yend) / self.l)
if printMessages:
print("phi values: " + str([phi0, phi1, phi2]))
print("theta values: " + str(theta))
if not self.isBetween([phi0, phi1, phi2], [self.minphi] * 3,
[self.maxphi] * 3):
if printMessages: print("phi out of range")
return False
if printMessages: print("in workspace")
return True
class JoystickController:
def __init__(self):
self.rate = 100 #[Hz]
self.sub_joy_right = rospy.Subscriber('joy_right', smsg.Joy,
self.cb_joy_right)
self.sub_joy_left = rospy.Subscriber('joy_left', smsg.Joy,
self.cb_joy_left)
self.joint_commands_pub = rospy.Publisher("/joint_commands",
Float32MultiArray,
queue_size=10)
self.xyz = [0, 0, -714.66] # Calibrated position
self.xyz_vel = [0.0, 0.0, 0.0]
self.iksolver = IKSolver()
self.wschecker = WorkspaceChecker()
rospy.Timer(rospy.Duration(1. / self.rate), self.update)
def cb_joy_right(self, joy):
self.xyz_vel[0] = 25. * joy.axes[1]
self.xyz_vel[1] = 25. * joy.axes[0]
def cb_joy_left(self, joy):
self.xyz_vel[2] = 25. * joy.axes[1]
def update(self, event):
print "Pos: " + str(self.xyz)
print "Vel: " + str(self.xyz_vel)
x = self.xyz[0] + self.xyz_vel[0] / self.rate
y = self.xyz[1] + self.xyz_vel[1] / self.rate
z = self.xyz[2] + self.xyz_vel[2] / self.rate
if self.wschecker.check([x, y, z]):
self.xyz = [x, y, z]
theta = self.iksolver.solve(self.xyz)
joint_commands_msg = Float32MultiArray()
joint_commands_msg.data = theta
self.joint_commands_pub.publish(joint_commands_msg)
class TrajectoryPlanner:
XYZ_VEL = 150.0
XYZ_ACCEL = 800.0
XYZ_VEL_FAST = 400.0
XYZ_ACCEL_FAST = 4900.0
pizzaradius = 130.0
shake_distance = 10.0
ZOFFSET = 60.0
MAX_PRESS_RADIUS = 55
NUM_SHAKES = 6
def __init__(self):
self.rate = 100.0 #[Hz]
self.iksolver = IKSolver()
self.wschecker = WorkspaceChecker()
self.xyz_pos_init = [0.0, 0.0, -714.66]
self.xyz_pos = [
0.0, 0.0, -714.66
] # Initial position right after calibration (maybe make this more robust)
self.xyz_goal_pos = [0.0, 0.0, -714.66]
self.running_task = False
self.running_trajectory = False
rospy.Subscriber("/place_topping", KFSPoseArray, self.place_topping_cb)
rospy.Subscriber("/shake_salt", KFSPoseArray, self.shake_salt_cb)
rospy.Subscriber("/press_dough", KFSPoseArray, self.press_dough_cb)
rospy.Subscriber("/push_pizza", KFSPose, self.push_pizza_cb)
rospy.Subscriber("/move_to", KFSPose, self.move_to_cb)
rospy.Subscriber("/finished_trajectory", Bool,
self.finished_trajectory_cb)
self.finished_task_pub = rospy.Publisher("/finished_task",
Bool,
queue_size=10)
self.trajectory_pub = rospy.Publisher("/trajectory",
DeltaTrajectory,
queue_size=10)
self.out_of_range = False
def finished_trajectory_cb(self, msg):
self.running_trajectory = not msg.data
if not self.running_trajectory:
self.xyz_pos = self.xyz_goal_pos
else:
print "Trajectory Follower published False for /finished_trajectory topic."
def place_topping_cb(self, msg):
if self.running_task:
print "Already working on a task"
return
self.running_task = True
print("Moving topping")
topping_xyz = [0, 0, 0]
topping_orientation = 0
topping_xyz[0] = msg.poses[0].position.x
topping_xyz[1] = msg.poses[0].position.y
topping_xyz[2] = msg.poses[0].position.z
topping_orientation = msg.poses[0].orientation
above_topping_xyz = [0, 0, 0]
above_topping_orientation = 0
above_topping_xyz[0] = msg.poses[0].position.x
above_topping_xyz[1] = msg.poses[0].position.y
above_topping_xyz[2] = msg.poses[0].position.z + self.ZOFFSET
above_topping_orientation = msg.poses[0].orientation
dest_xyz = [0, 0, 0]
dest_orientation = 0
dest_xyz[0] = msg.poses[1].position.x + 10 #1cm offset for pizza
dest_xyz[1] = msg.poses[1].position.y
dest_xyz[2] = msg.poses[1].position.z + 20
dest_orientation = msg.poses[1].orientation
above_dest_xyz = [0, 0, 0]
above_dest_orientation = 0
above_dest_xyz[0] = msg.poses[1].position.x
above_dest_xyz[1] = msg.poses[1].position.y
above_dest_xyz[2] = msg.poses[1].position.z + self.ZOFFSET
above_dest_orientation = msg.poses[1].orientation
self.generateMoveTo(above_topping_xyz, above_topping_orientation, True)
self.waitForTrajectoryToFinish()
if (self.out_of_range == False):
self.generateMoveTo(topping_xyz, topping_orientation, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(topping_xyz, topping_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(above_topping_xyz, above_topping_orientation,
False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(above_dest_xyz, above_dest_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(dest_xyz, dest_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(dest_xyz, dest_orientation, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(self.xyz_pos_init, 0, True)
self.waitForTrajectoryToFinish()
print("Moved topping!")
self.running_task = False
finished_task_msg = Bool()
if (self.out_of_range):
finished_task_msg.data = False
self.out_of_range = False
else:
finished_task_msg.data = True
self.finished_task_pub.publish(finished_task_msg)
def shake_salt_cb(self, msg):
if self.running_task:
print "Already working on a task"
return
self.running_task = True
shaker_xyz = [0, 0, 0]
shaker_orientation = 0
shaker_xyz[0] = msg.poses[0].position.x - 10.0
shaker_xyz[1] = msg.poses[0].position.y + 2.0
shaker_xyz[2] = msg.poses[0].position.z
shaker_orientation = msg.poses[0].orientation
above_shaker_xyz = [0, 0, 0]
above_shaker_orientation = 0
above_shaker_xyz[0] = msg.poses[0].position.x - 10.0
above_shaker_xyz[1] = msg.poses[0].position.y + 2.0
above_shaker_xyz[2] = msg.poses[0].position.z + self.ZOFFSET
above_shaker_orientation = msg.poses[0].orientation
dest_xyz = [0, 0, 0]
dest_orientation = 0
dest_xyz[0] = msg.poses[1].position.x
dest_xyz[1] = msg.poses[1].position.y
dest_xyz[2] = msg.poses[1].position.z
dest_orientation = msg.poses[1].orientation
above_dest_xyz = [0, 0, 0]
above_dest_orientation = 0
above_dest_xyz[0] = msg.poses[1].position.x
above_dest_xyz[1] = msg.poses[1].position.y
above_dest_xyz[2] = msg.poses[1].position.z + self.ZOFFSET + 60.0
above_dest_orientation = msg.poses[1].orientation
above_dest_left_xyz = [0, 0, 0]
above_dest_orientation = 0
above_dest_left_xyz[0] = msg.poses[1].position.x - self.shake_distance
above_dest_left_xyz[1] = msg.poses[1].position.y
above_dest_left_xyz[2] = msg.poses[1].position.z + self.ZOFFSET + 60.0
above_dest_left_orientation = msg.poses[1].orientation
above_dest_right_xyz = [0, 0, 0]
above_dest_right_orientation = 0
above_dest_right_xyz[0] = msg.poses[1].position.x + self.shake_distance
above_dest_right_xyz[1] = msg.poses[1].position.y
above_dest_right_xyz[2] = msg.poses[1].position.z + self.ZOFFSET + 60.0
above_dest_right_orientation = msg.poses[1].orientation
self.generateMoveTo(above_shaker_xyz, above_shaker_orientation, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(shaker_xyz, shaker_orientation, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(shaker_xyz, 0.7, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(shaker_xyz, 0.7, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(above_shaker_xyz, above_shaker_orientation, False)
self.waitForTrajectoryToFinish()
print('grabbed shaker')
self.generateMoveTo(above_dest_xyz, above_dest_orientation, False)
self.waitForTrajectoryToFinish()
'''
i=0
for i in range(self.NUM_SHAKES):
self.generateMoveToFAST(above_dest_left_xyz, dest_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveToFAST(above_dest_right_xyz, dest_orientation, False)
self.waitForTrajectoryToFinish()
print('im shook for the '+str(i)+' time')
'''
'''self.generateMoveToFAST(above_dest_left_xyz, dest_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveToFAST(above_dest_right_xyz, dest_orientation, False)
self.waitForTrajectoryToFinish()
print('im shook for the second time')'''
i = 0
shaker_shake_radius = 30
for i in range(0, 480, 15):
above_dest_circle = [
above_dest_xyz[0] +
shaker_shake_radius * math.cos(180 / math.pi * i),
above_dest_xyz[1] +
shaker_shake_radius * math.sin(180 / math.pi * i),
above_dest_xyz[2]
]
above_dest_circle_orientation = 0.0
self.generateMoveToFAST(above_dest_circle, 0, False)
self.waitForTrajectoryToFinish()
print('we have gone' + str(i) + 'around')
self.generateMoveTo(above_dest_xyz, above_dest_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(above_shaker_xyz, above_shaker_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(shaker_xyz, shaker_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(shaker_xyz, shaker_orientation, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(above_shaker_xyz, above_shaker_orientation, True)
self.waitForTrajectoryToFinish()
print("Shook salt!")
self.running_task = False
finished_task_msg = Bool()
finished_task_msg.data = True
self.finished_task_pub.publish(finished_task_msg)
def press_dough_cb(self, msg):
if self.running_task:
print "Already working on a task"
return
self.running_task = True
presser_xyz = [0, 0, 0]
presser_orientation = 0
presser_xyz[0] = msg.poses[0].position.x
presser_xyz[1] = msg.poses[0].position.y
presser_xyz[2] = msg.poses[0].position.z + 5
presser_orientation = msg.poses[0].orientation
above_presser_xyz = [0, 0, 0]
above_presser_orientation = 0
above_presser_xyz[0] = msg.poses[0].position.x
above_presser_xyz[1] = msg.poses[0].position.y
above_presser_xyz[2] = msg.poses[0].position.z + self.ZOFFSET + 75
above_presser_orientation = msg.poses[0].orientation
dest_xyz = [0, 0, 0]
dest_orientation = 0
dest_xyz[0] = msg.poses[1].position.x
dest_xyz[1] = msg.poses[1].position.y
dest_xyz[2] = msg.poses[1].position.z + 12
dest_orientation = msg.poses[1].orientation
above_dest_xyz = [0, 0, 0]
above_dest_orientation = 0
above_dest_xyz[0] = msg.poses[1].position.x
above_dest_xyz[1] = msg.poses[1].position.y
above_dest_xyz[2] = msg.poses[1].position.z + self.ZOFFSET
above_dest_orientation = msg.poses[1].orientation
self.generateMoveTo(above_presser_xyz, above_presser_orientation, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(presser_xyz, presser_orientation, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(presser_xyz, presser_orientation, False)
self.waitForTrajectoryToFinish()
print('grabbed presser')
self.generateMoveTo(above_presser_xyz, above_presser_orientation,
False)
self.waitForTrajectoryToFinish()
for i in range(4):
dest_xyz[0] = msg.poses[1].position.x
dest_xyz[1] = msg.poses[1].position.y
dest_xyz[2] = msg.poses[1].position.z + 20 - i * 2
above_dest_xyz[0] = msg.poses[1].position.x
above_dest_xyz[1] = msg.poses[1].position.y
above_dest_xyz[2] = msg.poses[1].position.z + self.ZOFFSET
self.generateMoveTo(above_dest_xyz, above_dest_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(dest_xyz, dest_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(above_dest_xyz, above_dest_orientation, False)
self.waitForTrajectoryToFinish()
r = 20
while (r < self.MAX_PRESS_RADIUS):
theta = 0.0
while theta < 360.0:
#r=self.pizzaradius*theta/360.0
x_off = r * math.cos(math.radians(theta))
y_off = r * math.sin(math.radians(theta))
print([x_off, y_off, theta])
dest_xyz[0] = msg.poses[1].position.x + x_off
dest_xyz[1] = msg.poses[1].position.y + y_off
dest_xyz[2] = msg.poses[1].position.z + 6
above_dest_xyz[0] = msg.poses[1].position.x + x_off
above_dest_xyz[1] = msg.poses[1].position.y + y_off
above_dest_xyz[2] = msg.poses[1].position.z + self.ZOFFSET
self.generateMoveTo(above_dest_xyz, above_dest_orientation,
False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(dest_xyz, dest_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(above_dest_xyz, above_dest_orientation,
False)
self.waitForTrajectoryToFinish()
theta += 30.0
r = r + 20.0
self.generateMoveTo(above_presser_xyz, above_presser_orientation,
False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(presser_xyz, presser_orientation, False)
self.waitForTrajectoryToFinish()
self.generateMoveTo(presser_xyz, presser_orientation, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(above_presser_xyz, above_presser_orientation, True)
self.waitForTrajectoryToFinish()
print("Pressed Dough!")
self.running_task = False
finished_task_msg = Bool()
finished_task_msg.data = True
self.finished_task_pub.publish(finished_task_msg)
def push_pizza_cb(self, msg):
if self.running_task:
print("Already working on a task")
return
self.running_task = True
dest_xyz = [0, 0, 0]
dest_orientation = 0
dest_xyz[0] = msg.position.x - self.pizzaradius
dest_xyz[1] = msg.position.y
dest_xyz[2] = msg.position.z + 10 #Go a little high on the pizza
dest_orientation = 0.6
above_dest_xyz = [0, 0, 0]
above_dest_xyz[0] = msg.position.x - self.pizzaradius
above_dest_xyz[1] = msg.position.y
above_dest_xyz[2] = msg.position.z + self.ZOFFSET
self.generateMoveTo(above_dest_xyz, 0, True)
self.waitForTrajectoryToFinish()
self.generateMoveTo(dest_xyz, dest_orientation, True)
self.waitForTrajectoryToFinish()
dest_xyz[0] = 215
self.generateMoveTo(dest_xyz, dest_orientation, True)
self.waitForTrajectoryToFinish()
print("Pushed Pizza!")
self.running_task = False
finished_task_msg = Bool()
finished_task_msg.data = True
self.finished_task_pub.publish(finished_task_msg)
def move_to_cb(self, msg):
if self.running_task:
print "Already working on a task"
return
self.running_task = True
print("Running move_to")
dest_xyz = [0, 0, 0]
dest_orientation = 0
dest_xyz[0] = msg.position.x
dest_xyz[1] = msg.position.y
dest_xyz[2] = msg.position.z
dest_orientation = msg.orientation
dest_open = msg.open
self.generateMoveTo(dest_xyz, dest_orientation, dest_open)
self.waitForTrajectoryToFinish()
self.running_task = False
finished_task_msg = Bool()
finished_task_msg.data = True
self.finished_task_pub.publish(finished_task_msg)
def generateMoveToFAST(self, xyz, orientation, gripper_open):
if self.running_trajectory:
print(
"Currently running a trajectory. Cannot call this function until goal position reached"
)
return
self.running_task = True
desired_xyz = np.array(xyz)
desired_rot = orientation
desired_open = gripper_open
xyz_initial = np.array(self.xyz_pos)
diff = desired_xyz - xyz_initial
d = np.linalg.norm(diff)
tr = self.XYZ_VEL_FAST / self.XYZ_ACCEL_FAST
tm = d / self.XYZ_VEL_FAST - tr
if tm < 0:
tr = math.sqrt(2 * d / self.XYZ_ACCEL_FAST)
tm = 0
t_tot = tm + 2 * tr
t_ = [i * 1. / self.rate for i in range(0, int(t_tot * self.rate))]
trajectory_msg = DeltaTrajectory()
joint_traj = []
gripper_traj = []
for t in t_:
j = JointPosition()
s = 0
if t < tr:
s = 0.5 * self.XYZ_ACCEL_FAST * t**2
elif t < tr + tm:
s = 0.5 * self.XYZ_ACCEL_FAST * tr**2 + (
t - tr) * self.XYZ_VEL_FAST
else:
s = 0.5 * self.XYZ_ACCEL_FAST * tr**2 + tm * self.XYZ_VEL_FAST + 0.5 * self.XYZ_ACCEL_FAST * (
tr**2 - (t - tm - 2 * tr)**2)
#Interpolates directly from A to B, no hopping motion
xyz_i = xyz_initial + diff * s / d if d != 0 else xyz_initial
if self.wschecker.check(xyz_i):
# DO IK
j.angles = self.iksolver.solve(xyz_i)
else:
# print error
print("error not in workspace")
return
g = GripperPosition()
g.open = gripper_open
g.angle = orientation
joint_traj.append(j)
gripper_traj.append(g)
if len(joint_traj) == 0 and len(gripper_traj) == 0:
j = JointPosition()
if self.wschecker.check(xyz_initial):
# DO IK
j.angles = self.iksolver.solve(xyz_initial)
else:
# print error
print("error not in workspace")
return
g = GripperPosition()
g.open = gripper_open
g.angle = orientation
joint_traj.append(j)
gripper_traj.append(g)
trajectory_msg.joint_trajectory = joint_traj
trajectory_msg.gripper_trajectory = gripper_traj
self.running_trajectory = True
self.xyz_goal_pos = list(desired_xyz)
self.trajectory_pub.publish(trajectory_msg)
def generateMoveTo(self, xyz, orientation, gripper_open):
if self.running_trajectory:
print(
"Currently running a trajectory. Cannot call this function until goal position reached"
)
return
self.running_task = True
desired_xyz = np.array(xyz)
desired_rot = orientation
desired_open = gripper_open
xyz_initial = np.array(self.xyz_pos)
diff = desired_xyz - xyz_initial
d = np.linalg.norm(diff)
tr = self.XYZ_VEL / self.XYZ_ACCEL
tm = d / self.XYZ_VEL - tr
if tm < 0:
tr = math.sqrt(2 * d / self.XYZ_ACCEL)
tm = 0
t_tot = tm + 2 * tr
t_ = [i * 1. / self.rate for i in range(0, int(t_tot * self.rate))]
trajectory_msg = DeltaTrajectory()
joint_traj = []
gripper_traj = []
for t in t_:
j = JointPosition()
s = 0
if t < tr:
s = 0.5 * self.XYZ_ACCEL * t**2
elif t < tr + tm:
s = 0.5 * self.XYZ_ACCEL * tr**2 + (t - tr) * self.XYZ_VEL
else:
s = 0.5 * self.XYZ_ACCEL * tr**2 + tm * self.XYZ_VEL + 0.5 * self.XYZ_ACCEL * (
tr**2 - (t - tm - 2 * tr)**2)
#Interpolates directly from A to B, no hopping motion
xyz_i = xyz_initial + diff * s / d if d != 0 else xyz_initial
if self.wschecker.check(xyz_i):
# DO IK
j.angles = self.iksolver.solve(xyz_i)
else:
# print error
print("error not in workspace")
self.out_of_range = True
return
g = GripperPosition()
g.open = gripper_open
g.angle = orientation
joint_traj.append(j)
gripper_traj.append(g)
if len(joint_traj) == 0 and len(gripper_traj) == 0:
j = JointPosition()
if self.wschecker.check(xyz_initial):
# DO IK
j.angles = self.iksolver.solve(xyz_initial)
else:
# print error
print("error not in workspace")
self.out_of_range = True
return
g = GripperPosition()
g.open = gripper_open
g.angle = orientation
joint_traj.append(j)
gripper_traj.append(g)
trajectory_msg.joint_trajectory = joint_traj
trajectory_msg.gripper_trajectory = gripper_traj
self.running_trajectory = True
self.xyz_goal_pos = list(desired_xyz)
self.trajectory_pub.publish(trajectory_msg)
def waitForTrajectoryToFinish(self):
while self.running_trajectory or self.xyz_pos != self.xyz_goal_pos:
rospy.sleep(1 / self.rate)
class TestTrajectoryGenerator:
def __init__(self):
self.rate = 100 #[Hz]
rospy.Subscriber("/finished_trajectory", Bool,
self.finished_trajectory_cb)
self.trajectory_pub = rospy.Publisher("/trajectory",
DeltaTrajectory,
queue_size=10)
self.iksolver = IKSolver()
self.wschecker = WorkspaceChecker()
time.sleep(1)
self.helix(150, 100, 200, 0.5)
def finished_trajectory_cb(self, msg):
print "Trajectory finished: " + str(msg.data)
def sine_wave(self,
amplitude=0.2,
frequency=0.2,
cycles=1,
include_gripper=False):
trajectory_msg = DeltaTrajectory()
joint_traj = []
gripper_traj = []
for t in range(0, int(cycles * self.rate / frequency)):
s = amplitude * math.sin(2 * math.pi * t * frequency / self.rate)
j = JointPosition()
j.angles = [s, s, s]
joint_traj.append(j)
g = GripperPosition()
g.angle = 0
g.open = False # if (t*frequency*1.0/self.rate)%1 > 0.5 else False
gripper_traj.append(g)
trajectory_msg.joint_trajectory = joint_traj
trajectory_msg.gripper_trajectory = gripper_traj
self.trajectory_pub.publish(trajectory_msg)
def helix(self, radius, pitch, h, freq):
initial_z = -714.66
current = [0, 0, initial_z]
initial = [radius, 0, initial_z]
trajectory_xyz = []
for t in range(0, 100):
xyz = [radius * t * 1. / 100, 0, initial_z]
trajectory_xyz.append(xyz)
for t in range(0, int(self.rate * h / pitch / freq)):
xyz = [
radius * math.cos(2 * math.pi * freq * t / self.rate),
radius * math.sin(2 * math.pi * freq * t / self.rate),
initial_z + freq * pitch * t / self.rate
]
trajectory_xyz.append(xyz)
for t in range(0, int(self.rate * h / pitch / freq)):
xyz = [
radius * math.cos(2 * math.pi * freq * t / self.rate),
radius * math.sin(2 * math.pi * freq * t / self.rate),
initial_z + h - freq * pitch * t / self.rate
]
trajectory_xyz.append(xyz)
for t in range(0, 100):
xyz = [radius * (1 - t * 1. / 100), 0, initial_z]
trajectory_xyz.append(xyz)
joint_traj = []
gripper_traj = []
for p in trajectory_xyz:
if self.wschecker.check(p):
theta = self.iksolver.solve(p)
j = JointPosition()
j.angles = theta
joint_traj.append(j)
g = GripperPosition()
gripper_traj.append(g)
else:
print "Trajectory not in workspace"
return False
trajectory_msg = DeltaTrajectory()
trajectory_msg.joint_trajectory = joint_traj
trajectory_msg.gripper_trajectory = gripper_traj
self.trajectory_pub.publish(trajectory_msg)