Commit 9c703498 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

reading joint states

parent 4e5c88f9
kinematics:
shoulder:
x: 0
y: 0
z: 0.1624898541114357
roll: -0
pitch: 0
yaw: -2.993110372051522e-08
upper_arm:
x: 0.000199748901491388
y: 0
z: 0
roll: 1.570191651791959
pitch: 0
yaw: 3.467196906109897e-06
forearm:
x: -0.4253389613409334
y: 0
z: 0
roll: 0.0005830482518067511
pitch: 0.0001777360512510511
yaw: 3.757108206717486e-07
wrist_1:
x: -0.3923171577167464
y: 7.90305563733867e-05
z: 0.1334493080654863
roll: 3.141000439600227
pitch: 3.140844198415325
yaw: 3.141590492022485
wrist_2:
x: 8.747259714909744e-05
y: -0.0998032513631754
z: -3.408488090727067e-05
roll: 1.571137847528105
pitch: 0
yaw: -1.455051852330621e-07
wrist_3:
x: 5.251979966562787e-06
y: 0.09980276859784269
z: -2.957416730143468e-06
roll: 1.570766694182787
pitch: 3.141592653589793
yaw: 3.141592441374947
hash: calib_17767642431275351403
\ No newline at end of file
import rospy
from trajectory_msgs.msg import (
JointTrajectoryPoint,
JointTrajectory
)
from sensor_msgs.msg import JointState
import time
class controller:
def __init__(self):
"""
Initiate connection to UR5
"""
self.node = rospy.init_node("py_controller", anonymous = False)
self.traj = rospy.Publisher('scaled_pos_traj_controller/command', JointTrajectory, queue_size=10)
self.joint_state = None
def exec_traj(self, traj):
"""
execute a JointTrajectory object
"""
rospy.loginfo("sending trajectroy to UR5")
pub.publish(traj)
def get_joints(self):
"""
Get current state of UR5
"""
def joint_state_setter(data):
self.joint_state = data.position
self.sb.unregister()
self.sb = rospy.Subscriber("joint_states", JointState, joint_state_setter)
time.sleep(0.1)
return self.joint_state
if __name__=="__main__":
print "Testing joint angle"
con = controller()
print con.get_joints()
import time
import numpy as np
import gcode2contour as gc
import rospy
from trajectory_msgs.msg import (
JointTrajectoryPoint,
JointTrajectory
)
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
......@@ -17,4 +49,9 @@ if __name__ == "__main__":
fname = "benchy.gcode"
contours = gc.decode_gcode(fname)
print_contours(contours)
# Calibrate system
# TODO
# Apply calibration offset to contours
# TODO
#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