Commit 5464f641 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

bugfixes and cleanup

parent 3d4b2d9e
from copy import copy, deepcopy
from pynput import keyboard
from enum import Enum
from utils import ur_kinematics
from math import pi
import time
import warnings
from utils import *
import sys
......@@ -76,7 +74,6 @@ class controller:
self.joint_state = None
self.calibration = None
self._temp_calib = None
self.kin = ur_kinematics("UR5e")
time.sleep(1)
# initialise MoveIt commander
......@@ -101,7 +98,8 @@ class controller:
def fk(self, joints):
"""
compute forward kinematics
This uses the /compute_fk rosservice - which doesnt work
Using /compute_fk rosservice
- which doesnt work
"""
rospy.wait_for_service('compute_fk')
......@@ -111,7 +109,7 @@ class controller:
rospy.logerr("Service call failed: %s"%e)
req = GetPositionFKRequest()
req.header.frame_id = 'base'
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.position = joints
......@@ -126,7 +124,7 @@ class controller:
- this one actually works
"""
if start_angle = None:
if start_angle == None:
start_angle = con.group.get_current_joint_values()
rospy.wait_for_service('compute_ik')
......@@ -165,37 +163,10 @@ class controller:
"""
self.group.go(joint_angles)
# now = self.robot.get_current_state().joint_state.position
# max_dTheta = max([abs(now[a]-joint_angles[a]) for a in range(len(now))])
# total_time = max_dTheta/max_speed
#
# t = self.empty_traj()
# p1 = JointTrajectoryPoint()
# p1.positions = copy(now)
# p1.time_from_start = rospy.Time(0)
# p2 = JointTrajectoryPoint()
# p2.positions = copy(tuple(joint_angles))
# p2.time_from_start = rospy.Time(total_time)
# t.points.append(p1)
# t.points.append(p2)
#
# print t
# self.exec_traj(t)
# def get_joints(self):
# """
# Get current state of UR5
# """
# def joint_state_setter(data):
# self.joint_state = data
# self.sb.unregister()
#
# self.sb = rospy.Subscriber("joint_states", JointState, joint_state_setter)
#
# time.sleep(0.1)
# return self.joint_state.position
def get_joints(self):
return self.group.get_current_joint_values()
def empty_traj(self):
"""
......@@ -246,7 +217,7 @@ 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.get_joints())
return self._temp_calib.push_parameter(self.group.get_current_joint_values())
def _internal_on_release(self, key):
......
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