Commit a6c34938 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

forward kinematics working, executing contour trajectory

parent e4ddce3f
......@@ -6,6 +6,7 @@ import time
import warnings
import sys
import gcode2contour as gc
from utils import clean_contour
import rospy
......@@ -22,6 +23,7 @@ from moveit_commander.conversions import pose_to_list
from moveit_msgs.srv import GetPositionFK, GetPositionFKRequest, GetPositionFKResponse
from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest, GetPositionIKResponse
from moveit_msgs.msg import RobotState
from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
class CalibrationType(Enum):
......@@ -103,7 +105,6 @@ class controller:
"""
compute forward kinematics
Using /compute_fk rosservice
- which doesnt work
"""
rospy.wait_for_service('compute_fk')
......@@ -115,7 +116,7 @@ class controller:
req = GetPositionFKRequest()
req.header.frame_id = 'world'
req.fk_link_names = ['ee_link']
req.robot_state.joint_state.name = self.group.get_joints()
req.robot_state.joint_state.name = self.group.get_active_joints()
req.robot_state.joint_state.position = joints
return moveitFK.call(req)
......@@ -150,9 +151,15 @@ class controller:
def exec_traj(self, traj):
"""
execute a JointTrajectory object
NOTE: If not currently at starting point, this will fail
"""
rospy.loginfo("sending trajectroy to UR5")
self.traj.publish(traj)
#self.traj.publish(traj)
plan = RobotTrajectory()
plan.joint_trajectory = traj
self.group.execute(plan)
def move_to(self, joint_angles, max_speed = pi/100):
......@@ -172,12 +179,13 @@ class controller:
def get_joints(self):
return self.group.get_current_joint_values()
def empty_traj(self):
"""
Empty trajectory with Joint names filled in
"""
t = JointTrajectory()
t.joint_names = self.group.get_joints()
t.joint_names = self.group.get_active_joints()
return t
......@@ -187,14 +195,15 @@ class controller:
"""
# step 1 get current pose
now = self.group.get_current_pose().pose
new = deepcopy(now)
# step 2 shift pose
now.position.x += dx
now.position.y += dy
now.position.z += dz
new.position.x += dx
new.position.y += dy
new.position.z += dz
# step 3 compute cartesian plan
(plan, frac) = self.group.compute_cartesian_path([now], 0.01, 0.0)
(plan, frac) = self.group.compute_cartesian_path([now, new], 0.01, 0.0)
# step 4 execute plan
self.group.execute(plan)
......@@ -283,6 +292,7 @@ class controller:
4. save points and timestamp in trajectory
"""
clean_contour(contour)
# Get calibration pose:
p0 = self.calibration.cb_pose[0]
p_last = deepcopy(p0)
......@@ -309,31 +319,4 @@ class controller:
if __name__=="__main__":
########################################
print "Testing joint angle subscriber"
con = controller()
# print con.get_joints()
########################################
# print "Testing Joint Trajectory controller"
#
# 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)
#
# p1 = JointTrajectoryPoint()
# p1.positions = copy(js1)
# p1.time_from_start = rospy.Time(0)
# p2 = JointTrajectoryPoint()
# p2.positions = copy(js2)
# p2.time_from_start = rospy.Time(3)
# p3 = JointTrajectoryPoint()
# p3.positions = copy(js1)
# p3.time_from_start = rospy.Time(6)
# t.points.append(p1)
# t.points.append(p2)
# t.points.append(p3)
#
# print t
# con.exec_traj(t)
......@@ -26,4 +26,5 @@ if __name__ == "__main__":
traj = con.contour2traj(contours[0])
# Execute trajectory
con.move_to(traj.points[0].positions)
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