Commit 29816f7d authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

work towards executing contour execution dry

parent 5464f641
This diff is collapsed.
......@@ -41,10 +41,11 @@ class Calibration:
assert(isinstance(calibration_type, CalibrationType))
self.cb_t = calibration_type
self.cb_parameters = []
self.cb_joints = []
self.cb_pose = []
self.complete = False
def push_parameter(self, param):
def push_parameter(self, joints, pose):
"""
Save the joint angles to reach particular point in grid
param should be a list of floats (joint angles) (not asserted)
......@@ -217,7 +218,10 @@ class controller:
self._internal_move_endpoint(0, 0, -0.01)
elif key == keyboard.Key.enter:
# save current pose into calibaration
return self._temp_calib.push_parameter(self.group.get_current_joint_values())
return self._temp_calib.push_parameter(
self.group.get_current_joint_values(),
self.group.get_current_pose()
)
def _internal_on_release(self, key):
......@@ -247,6 +251,18 @@ class controller:
self.calibration = self._temp_calib
def contour2traj(self, contour):
"""
Convert a contour to a arm trajectory:
1. shift points to calibration
2. convert positions to poses
3. get IK for each pose
4. save points and timestamp in trajectory
"""
pass
if __name__=="__main__":
########################################
......
import time
import numpy as np
import gcode2contour as gc
from controller import controller, Calibration, CalibrationType
import rospy
from trajectory_msgs.msg import (
......@@ -9,49 +10,18 @@ from trajectory_msgs.msg import (
)
def contour2traj(c):
"""
Converts contours to JointTrajectory objects
Assumes that the contour XYZ locations have already been offset
"""
# IK for each point in the trajectory
# choose valid starting IK -> closest to current position
# create Trajectory using IK.
traj = JointTrajectory()
for name in planner.joint_names[robot]:
traj_msg.joint_names.append(name)
for x in c.pos:
point = JointTrajectoryPoint()
point.positions = copy([0,0,0,0,0,0])# TODO change to get the relevant IK
point.time_from_start = rospy.Duration(0) # TODO get time out of contour
pass
def print_contours(contours):
"""
takes a list contours and prints them
"""
#TODO current pose - get from
for c in contours:
t = contour2traj(c)#, current_pose)
# Execute Trajectory
pass
if __name__ == "__main__":
# convert gcode file to contours
fname = "benchy.gcode"
contours = gc.decode_gcode(fname)
# Load controller
con = controller()
# Calibrate system
# TODO
# Apply calibration offset to contours
# TODO
con.calibrate()
# convert (a) contour to arm trajectory
traj = con.contour2traj(contours[0])
#print_contours(contours)
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