def __init__(self): self.j = Jaco() self.listener = tf.TransformListener() self.current_joint_angles = [0] * 7 self.tfBuffer = tf2_ros.Buffer() self.listen = tf2_ros.TransformListener(self.tfBuffer) self.velocity_pub = rospy.Publisher( '/j2n6s300_driver/in/cartesian_velocity', PoseVelocity, queue_size=1) self.joint_angles_sub = rospy.Subscriber( "/j2n6s300_driver/out/joint_angles", JointAngles, self.callback) self.obj_det_sub = rospy.Subscriber('/finger_sensor/obj_detected', FingerDetect, self.set_obj_det) self.fingetouch_finger_2_sub = rospy.Subscriber( '/finger_sensor/touch', FingerTouch, self.set_touch) 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.touch_finger_1 = False self.touch_finger_3 = False self.calibrated = False
def __init__(self): self.j = Jaco() self.listener = tf.TransformListener() self.current_joint_angles = [0] * 7 self.tfBuffer = tf2_ros.Buffer() self.listen = tf2_ros.TransformListener(self.tfBuffer) self.velocity_pub = rospy.Publisher( '/j2n6s300_driver/in/cartesian_velocity', PoseVelocity, queue_size=1) self.joint_angles_sub = rospy.Subscriber( "/j2n6s300_driver/out/joint_angles", JointAngles, self.callback) self.obj_det_sub = rospy.Subscriber('/finger_sensor/obj_detected', FingerDetect, self.set_obj_det) self.fingetouch_finger_2_sub = rospy.Subscriber( '/finger_sensor/touch', FingerTouch, self.set_touch) 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.bump_det_sub = rospy.Subscriber("/finger_sensor/fai", FingerFAI, self.detect_Bump) self.tool_wrench_sub = rospy.Subscriber( "/j2n6s300_driver/out/tool_wrench", WrenchStamped, self.tool_wrench) self.tool_pose_sub = rospy.Subscriber("/j2n6s300_driver/out/tool_pose", PoseStamped, self.tool_pose) self.sensor_sub = rospy.Subscriber('/sensor_values', Int32MultiArray, self.handle_sensor) self.obj_det = False self.sensor_values = [] self.touch_finger_1 = False self.touch_finger_3 = False self.calibrated = False self.bump_finger_1 = 0 self.bump_finger_2 = 0 self.tool_wrench_x = 0 self.tool_wrench_y = 0 self.tool_wrench_z = 0 self.angles = [] self.sensorValues_1 = [] self.sensorValues_2 = [] self.tool_pose_ = [0] * 7
def moveRobot(env, robot, dof, dx, dy, dz): #tp = TrajoptPlanner.TrajoptPlanner(env) if len(dof) == 7: dof = np.append(dof, np.array([1, 1, 1])) with robot: robot.SetDOFValues(dof) transform = robot.GetActiveManipulator().GetEndEffectorTransform() new_transform = move(transform, dx, dy, dz) print("original", transform) print("new", new_transform) #path = tp.PlanToIK(robot, new_transform) #return path.GetAllWaypoints2D()[-1] archie = Jaco(robot) return archie.FindIKSolution(new_transform)