Commit 123b72b0 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

contour2traj and bugfixes

parent 5e85f129
......@@ -5,6 +5,7 @@ from math import pi
import time
import warnings
import sys
import gcode2contour as gc
import rospy
......@@ -51,13 +52,15 @@ class Calibration:
param should be a list of floats (joint angles) (not asserted)
"""
if len(self.cb_parameters) == self.cb_t.value:
if len(self.cb_joints) == self.cb_t.value:
warnings.warn("all parameters are already set, ignoring this input")
return False
self.cb_parameters.append(param)
self.cb_pose.append(pose)
self.cb_joints.append(joints)
if len(self.cb_parameters) == self.cb_t.value:
if len(self.cb_joints) == self.cb_t.value:
self.complete = True
print "Calibration Complete"
return False
......@@ -126,7 +129,7 @@ class controller:
"""
if start_angle == None:
start_angle = con.group.get_current_joint_values()
start_angle = self.group.get_current_joint_values()
rospy.wait_for_service('compute_ik')
try:
......@@ -137,7 +140,7 @@ class controller:
req = GetPositionIKRequest()
req.ik_request.ik_link_name = 'ee_link'
req.ik_request.group_name = 'manipulator'
req.ik_request.robot_state.joint_state.name = con.group.get_joints()
req.ik_request.robot_state.joint_state.name = self.group.get_joints()
req.ik_request.robot_state.joint_state.position = start_angle
req.ik_request.pose_stamped = pose
......@@ -167,14 +170,14 @@ class controller:
def get_joints(self):
return self.group.get_current_joint_values()
return self.group.get_current_joint_values()
def empty_traj(self):
"""
Empty trajectory with Joint names filled in
"""
t = JointTrajectory()
t.joint_names = self.joint_state.name
t.joint_names = self.group.get_joints()
return t
......@@ -217,7 +220,7 @@ class controller:
elif key == keyboard.Key.page_down:
self._internal_move_endpoint(0, 0, -0.01)
elif key == keyboard.Key.enter:
# save current pose into calibaration
# save current pose into calibration
return self._temp_calib.push_parameter(
self.group.get_current_joint_values(),
self.group.get_current_pose()
......@@ -266,8 +269,8 @@ class controller:
self.group.set_pose_target(p)
plan = self.group.go(wait=True)
group.stop() # residual motion
group.clear_pose_targets()
self.group.stop() # residual motion
self.group.clear_pose_targets()
return self.group.get_current_pose()
......@@ -281,23 +284,27 @@ class controller:
"""
# Get calibration pose:
p0 = self.calibaration.cb_pose[0]
p0 = self.calibration.cb_pose[0]
p_last = deepcopy(p0)
joints = []
for position in contour.pos:
t = self.empty_traj()
for i, position in enumerate(contour.pos):
pose = deepcopy(p0)
pose[-1].pose.position.x += position[0]/1e3
pose[-1].pose.position.y += position[1]/1e3
pose[-1].pose.position.z += position[2]/1e3
pose.pose.position.x += position[0]/1e3
pose.pose.position.y += position[1]/1e3
pose.pose.position.z += position[2]/1e3
# Get IK for all the poses
j_s = self.ik(pose, start_angle = p_last)
j_s = self.ik(pose) #, start_angle = p_last)
p_last = j_s.solution.joint_state.position
joints.append(p_last)
# Append to trajectory
jtp = JointTrajectoryPoint()
jtp.positions = deepcopy(p_last)
jtp.time_from_start = rospy.Time(contour.time[i])
t.points.append(jtp)
return t
if __name__=="__main__":
......@@ -312,7 +319,6 @@ if __name__=="__main__":
########################################
# print "Testing Joint Trajectory controller"
#
# t = con.empty_traj()
# js1 = (-1.3825197219848633,-1.7300764522948207,1.800080776214599,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
# js2 = (-1.3825197219848633,-1.7300764522948207,2.323414109547933,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
#
......
......@@ -20,8 +20,10 @@ if __name__ == "__main__":
con = controller()
# Calibrate system
con.calibrate()
con.calibrate(CalibrationType.centre)
# convert (a) contour to arm trajectory
traj = con.contour2traj(contours[0])
# Execute trajectory
con.exec_traj(traj)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment