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 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)
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