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

switch to moveit interface, fk implemented but not working in simulation

parent 7ac8e708
......@@ -20,6 +20,9 @@ from trajectory_msgs.msg import (
from sensor_msgs.msg import JointState
from std_msgs.msg import String
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
class CalibrationType(Enum):
......@@ -88,13 +91,47 @@ class controller:
queue_size=20)
# Potentially useful but not used right now
# self.planning_frame = group.get_planning_frame()
# self.eef_link = group.get_end_effector_link()
# self.group_names = robot.get_group_names()
# self.planning_frame = self.group.get_planning_frame()
self.ee_link = self.group.get_end_effector_link()
# self.group_names = self.robot.get_group_names()
print self.robot.get_current_state()
def fk(self, joints):
"""
compute forward kinematics
"""
rospy.wait_for_service('compute_fk')
try:
moveitFK = rospy.ServiceProxy('compute_fk', GetPositionFK)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s"%e)
req = GetPositionFKRequest()
req.header.frame_id = 'base_link'
req.fk_link_names = ['ee_link']
req.robot_state.joint_state.name = self.group.get_joints()
req.robot_state.joint_state.position = joints
return moveitFK.call(req)
def ik(self, pose):
"""
Inverse Kinematics
"""
rospy.wait_for_service('compute_ik')
try:
moveitIK = rospy.ServiceProxy('compute_ik', GetPositionFK)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s"%e)
pass
def exec_traj(self, traj):
"""
execute a JointTrajectory object
......@@ -114,36 +151,37 @@ class controller:
max speed is in rad/s, deafault 10s to do a 180
"""
now = self.get_joints()
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
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 empty_traj(self):
......@@ -232,27 +270,27 @@ if __name__=="__main__":
con = controller()
print con.get_joints()
# print con.get_joints()
########################################
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)
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)
# 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)
#
# 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)
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