class GotoObject(smach.State):
def __init__(self,frame):
smach.State.__init__(self, outcomes=['there','no_tf_found'])
self.frame = frame
self.jn = Jaco()
self.listen = tf.TransformListener()
def execute(self,userdata):
if self.listen.frameExists("/root") and self.listen.frameExists(self.frame):
self.listen.waitForTransform('/root',self.frame,rospy.Time(),rospy.Duration(100.0))
t = self.listen.getLatestCommonTime("/root", self.frame)
translation, quaternion = self.listen.lookupTransform("/root", self.frame, t)
translation = list(translation)
quaternion = list(quaternion)
msg = self.make_pose_stamped_msg(translation,quaternion)
self.jn.move_ik(msg)
return 'there'
else:
return 'no_tf_found'
def make_pose_stamped_msg(self,translation,orientation,frame='/j2n6a300_link_base'):
msg = PoseStamped()
msg.header.frame_id = frame
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = translation[0]
msg.pose.position.y = translation[1]
msg.pose.position.z = translation[2]
msg.pose.orientation.x = orientation[0]
msg.pose.orientation.y = orientation[1]
msg.pose.orientation.z = orientation[2]
return msg
def __init__(self):
smach.State.__init__(self,
outcomes=['Ready', 'Start'],
input_keys=['tower_size_in'],
output_keys=['tower_size_out'])
self.jn = Jaco()
self.jgn = JacoGripper()
def __init__(self,fingers,search='back_forth_search_xy'):
smach.State.__init__(self, outcomes=['found', 'not_found'])
self.finger_detect_sub = rospy.Subscriber('/finger_sensor/obj_detected',
FingerDetect,
self.set_finger_detect)
self.finger_detect = [None, None, None]
self.jn = Jaco()
self.detect_goal = [True] * len(fingers)
self.fingers = fingers
self.listen = tf.TransformListener()
self.transformer = tf.TransformerROS()
searches = {'back_forth_search_xy':self.back_forth_search_xy,
'down_z': self.down_z}
self.search_motion = searches[search]
def __init__(self):
self.j = Jaco()
self.listen = tf.TransformListener()
self.current_joint_angles = [0] * 6
self.velocity_pub = rospy.Publisher(
'/j2n6a300_driver/in/cartesian_velocity',
PoseVelocity,
queue_size=1)
self.obj_det_sub = rospy.Subscriber('/finger_sensor/obj_detected',
Bool, self.set_obj_det)
self.finger_m_touch_sub = rospy.Subscriber(
'/finger_sensor_middle/touch', Bool, self.set_m_touch)
self.finger_r_touch_sub = rospy.Subscriber(
'/finger_sensor_right/touch', Bool, self.set_r_touch)
self.joint_angles_sub = rospy.Subscriber(
"/j2n6a300_driver/out/joint_angles", JointAngles, self.callback)
self.calibrate_obj_det_pub = rospy.Publisher(
"/finger_sensor/calibrate_obj_det", Bool, queue_size=1)
self.calibrate_obj_det_sub = rospy.Subscriber(
"/finger_sensor/obj_det_calibrated", Bool, self.set_calibrated)
self.obj_det = False
self.m_touch = False
self.r_touch = False
self.calibrated = False
class StackingStart(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['Ready','Start'],
input_keys=['tower_size_in'],
output_keys=['tower_size_out'])
self.jn = Jaco()
self.jgn = JacoGripper()
def execute(self,userdata):
self.jgn.set_position([0.0,0.0,0.0])
if userdata.tower_size_in == 0:
self.jn.home()
userdata.tower_size_out = userdata.tower_size_in + 1
return 'Start'
else:
self.jn.home()
userdata.tower_size_out = userdata.tower_size_in + 1
return 'Ready'
class SearchObject(smach.State):
def __init__(self,fingers,search='back_forth_search_xy'):
smach.State.__init__(self, outcomes=['found', 'not_found'])
self.finger_detect_sub = rospy.Subscriber('/finger_sensor/obj_detected',
FingerDetect,
self.set_finger_detect)
self.finger_detect = [None, None, None]
self.jn = Jaco()
self.detect_goal = [True] * len(fingers)
self.fingers = fingers
self.listen = tf.TransformListener()
self.transformer = tf.TransformerROS()
searches = {'back_forth_search_xy':self.back_forth_search_xy,
'down_z': self.down_z}
self.search_motion = searches[search]
def set_finger_detect(self,msg):
self.finger_detect[0] = msg.finger1
self.finger_detect[1] = msg.finger2
self.finger_detect[2] = msg.finger3
def create_pose_velocity_msg(self,cart_velo,transform=False):
if transform:
if self.listen.frameExists("/root") and self.listen.frameExists("/j2n6a300_end_effector"):
self.listen.waitForTransform('/root',"/j2n6a300_end_effector",rospy.Time(),rospy.Duration(100.0))
t = self.listen.getLatestCommonTime("/root", "/j2n6a300_end_effector")
translation, quaternion = self.listen.lookupTransform("/root", "/j2n6a300_end_effector", t)
transform = self.transformer.fromTranslationRotation(translation, quaternion)
transformed_vel = np.dot(transform,(cart_velo[0:3]+[1.0]))
print(transformed_vel)
cart_velo[0:3] = transformed_vel[0:3]
msg = PoseVelocity(
twist_linear_x=cart_velo[0],
twist_linear_y=cart_velo[1],
twist_linear_z=cart_velo[2],
twist_angular_x=cart_velo[3],
twist_angular_y=cart_velo[4],
twist_angular_z=cart_velo[5])
return msg
def execute(self,userdata):
if np.all(np.array(self.finger_detect)[[self.fingers]] == np.array(self.detect_goal)):
return 'found'
else:
found = self.search_motion()
return found
def back_forth_search_xy(self):
rate = rospy.Rate(100)
for i in range(100):
if np.all(np.array(self.finger_detect) == np.array(self.detect_goal)):
return 'found'
msg = self.create_pose_velocity_msg([0.0,-0.05,0.0,0.0,0.0,0.0])
self.jn.kinematic_control(msg)
rate.sleep()
for i in range(200):
if np.all(np.array(self.finger_detect)[[self.fingers]] == np.array(self.detect_goal)):
return 'found'
msg = self.create_pose_velocity_msg([0.0,0.05,0.0,0.0,0.0,0.0])
self.jn.kinematic_control(msg)
rate.sleep()
for i in range(100):
if np.all(np.array(self.finger_detect) == np.array(self.detect_goal)):
return 'found'
msg = self.create_pose_velocity_msg([0.0,-0.05,0.0,0.0,0.0,0.0])
self.jn.kinematic_control(msg)
rate.sleep()
return 'not_found'
def down_z(self):
rate = rospy.Rate(100)
while True:
if np.any(np.array(self.finger_detect)[[self.fingers]] == np.array(self.detect_goal)):
return 'found'
choices = np.array([0.0,0.05,-0.05,0.1,-0.1])
x = np.random.choice(choices)
y = np.random.choice(choices)
z = -0.01
msg = self.create_pose_velocity_msg([x,y,z,0.0,0.0,0.0])
self.jn.kinematic_control(msg)
rate.sleep()
return 'not_found'
def __init__(self,frame):
smach.State.__init__(self, outcomes=['there','no_tf_found'])
self.frame = frame
self.jn = Jaco()
self.listen = tf.TransformListener()
#!/usr/bin/env python
import rospy
import numpy as np
import tf
from pap.jaco import Jaco, JacoGripper
import commands
import time
import actionlib
import kinova_msgs.msg
from std_msgs.msg import Bool
if __name__ == '__main__':
rospy.init_node('cal_pos')
jn = Jaco()
jn.move_joints([
203.327209473, 279.609375, 143.161758423, 149.386367798, 119.659095764,
111.204551697
])