self.touch_centroids[self.current_index] = np.array([x,y,z])
self.capture_centroid(self.object_frame)
self.current_index += 1
def publish_transform(name, pos):
broadcast = tf.TransformBroadcaster()
while(True):
broadcast.sendTransform(tuple(pos), [0,0,0,1],
rospy.Time.now(),
name,
"root")
rospy.sleep(1)
if __name__ == '__main__':
n = PickAndPlaceNode(FingerSensorBaxter, 'right')
home = PoseStamped(
Header(0, rospy.Time(0), n.robot.base),
Pose(Point(0.60558, -0.24686, 0.093535),
Quaternion(0.99897, -0.034828, 0.027217, 0.010557)))
n.robot.move_ik(home)
#thread.start_new_thread(publish_transform, ("block_0_touch", [.2,.2,.2]))
raw_input("Press Enter to find cubes...")
#get block 0 location
f = open('centroid_storage.txt', 'w')
cube = 0
for i in range(start_num,start_num+num_cubes):
n.robot.object_frame = cube_name + str(i)
if(n._pickFrame(n.robot.object_frame)):
r.sleep()
# We're usually over by now, so let's go back a little bit
r = rospy.Rate(20)
for i in range(2):
v_cartesian = self._vector_to((0, -copysign(0.005, delta), 0))
v_joint = self.compute_joint_velocities(v_cartesian)
self.limb.set_joint_velocities(v_joint)
r.sleep()
self.limb.set_joint_velocities(self.compute_joint_velocities([0] * 6))
rospy.loginfo("centered")
rospy.sleep(0.5)
#self.move_ik(pose)
#rospy.sleep(0.5)
self.gripper.open()
rospy.sleep(0.5)
self.move_ik(preplace_pose)
if __name__ == '__main__':
n = PickAndPlaceNode(FingerSensorBaxter, 'right')
home = PoseStamped(
Header(0, rospy.Time(0), n.robot.base),
Pose(Point(0.60558, -0.24686, 0.093535),
Quaternion(0.99897, -0.034828, 0.027217, 0.010557)))
n.robot.move_ik(home)
raw_input("Press Enter to continue...")
n.robot.scan_cup()
def run_auto(self):
r = rospy.Rate(20)
while not rospy.is_shutdown():
for i in range(20):
v = (0, 0, 0.02)
v_cartesian = self.bx._vector_to(v)
v_joint = self.bx.compute_joint_velocities(v_cartesian)
self.bx.limb.set_joint_velocities(v_joint)
r.sleep()
for i in range(20):
v = (0, 0, -0.02)
v_cartesian = self.bx._vector_to(v)
v_joint = self.bx.compute_joint_velocities(v_cartesian)
self.bx.limb.set_joint_velocities(v_joint)
r.sleep()
if __name__ == '__main__':
if False:
p = Placement()
# p.run_manual()
p.run_auto()
smart = True
#1/0
if smart:
n = PickAndPlaceNode(SmartBaxter, 'left')
else:
n = PickAndPlaceNode(Jaco)
# n = PickAndPlaceNode(Baxter('left'))
rospy.spin()
#! /usr/bin/env python
import rospy
from pap.manager import PickAndPlaceNode
from perception.msg import identified_object
from pap.jaco import Jaco
if __name__ == '__main__':
#n = PickAndPlaceNode('left')
n = PickAndPlaceNode(Jaco)
rospy.spin()
#! /usr/bin/env python
import rospy
from pap.manager import PickAndPlaceNode
if __name__ == '__main__':
n = PickAndPlaceNode('left')
rospy.spin()
self.touch_centroids[self.current_index] = np.array([x, y, z])
self.capture_centroid(self.object_frame)
self.current_index += 1
#visualizes the touch centroid in RVIZ for debugging/verification purposes
def publish_transform(name, pos):
broadcast = tf.TransformBroadcaster()
while (True):
broadcast.sendTransform(tuple(pos), [0, 0, 0, 1], rospy.Time.now(),
name, "root")
rospy.sleep(1)
if __name__ == '__main__':
n = PickAndPlaceNode(FingerSensorBaxter,
'right') #right is for the right arm
home = PoseStamped(
Header(0, rospy.Time(0), n.robot.base),
Pose(Point(0.60558, -0.24686, 0.093535),
Quaternion(0.99897, -0.034828, 0.027217, 0.010557)))
n.robot.move_ik(home)
num_cubes = int(
raw_input("Enter number of cubes (names should start at block_0!):"))
raw_input("Press Enter to find cubes...")
print("\nSearching for " + str(num_cubes) + " cubes...\n")
if (save_to_file): f = open('centroid_storage.txt', 'w')
cube = 0
for i in range(start_num, start_num + num_cubes):
n.robot.object_frame = cube_name + str(i)