def __init__(self, controller_commander, urdf_xml_string, srdf_xml_string,
camera_image_topic, camera_trigger_topic, camera_info_topic):
self.controller_commander = controller_commander
self.planner = Planner(controller_commander, urdf_xml_string,
srdf_xml_string)
self.tf_listener = PayloadTransformListener()
self.camera_info = self.get_camera_info(camera_info_topic)
self.img_queue = Queue.Queue(1)
self.camera_trigger = rospy.ServiceProxy(camera_trigger_topic, Trigger)
self.camera_sub = rospy.Subscriber(camera_image_topic, Image,
self._ros_img_cb)
self.controller_state_sub = rospy.Subscriber("controller_state",
controllerstate,
self.ft_cb)
self.FTdata = None
self.ft_flag = False
self.FTdata_0 = self.FTdata
# Compliance controller parameters
self.F_d_set1 = -100
self.F_d_set2 = -200
self.Kc = 0.00002
self.client = actionlib.SimpleActionClient(
"joint_trajectory_action", FollowJointTrajectoryAction)
self.K_pbvs = 0.25
def __init__(self, disable_ft=False):
self.urdf = URDF.from_parameter_server()
self.overhead_vision_client = actionlib.SimpleActionClient(
"recognize_objects", ObjectRecognitionAction)
self.execute_trajectory_action = actionlib.SimpleActionClient(
"execute_trajectory", ExecuteTrajectoryAction)
self.rapid_node = rapid_node_pkg.RAPIDCommander()
self.controller_commander = controller_commander_pkg.ControllerCommander(
)
self.state = 'init'
self.current_target = None
self.current_payload = None
self.available_payloads = {
'leeward_mid_panel': 'leeward_mid_panel',
'leeward_tip_panel': 'leeward_tip_panel'
}
self.desired_controller_mode = self.controller_commander.MODE_AUTO_TRAJECTORY
self.speed_scalar = 1.0
self.disable_ft = disable_ft
self.tf_listener = PayloadTransformListener()
self._process_state_pub = rospy.Publisher("process_state",
ProcessState,
queue_size=100,
latch=True)
self.publish_process_state()
self.update_payload_pose_srv = rospy.ServiceProxy(
"update_payload_pose", UpdatePayloadPose)
self.get_payload_array_srv = rospy.ServiceProxy(
"get_payload_array", GetPayloadArray)
self.plan_dictionary = {}
def __init__(self):
#Subscribe to controller_state
self.controller_state_sub = rospy.Subscriber("controller_state", controllerstate, self.callback)
self.last_ros_image_stamp = rospy.Time(0)
self.goal_handle=None
self.listener = PayloadTransformListener()
self.rapid_node = rapid_node_pkg.RAPIDCommander()
self.controller_commander = controller_commander_pkg.ControllerCommander()
self.object_commander = ObjectRecognitionCommander()
# Initilialize aruco boards and parameters
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_ARUCO_ORIGINAL)
self.parameters = cv2.aruco.DetectorParameters_create()
self.parameters.cornerRefinementMethod=cv2.aruco.CORNER_REFINE_SUBPIX
self.parameters.adaptiveThreshWinSizeMax=30
self.parameters.adaptiveThreshWinSizeStep=7
# ================== Cam 636
# --- Subscribe to Gripper camera node for image acquisition
#TODO: Take in camera names and make subscribers and triggers accordingly
self.ros_gripper_2_img_sub = rospy.Subscriber('/gripper_camera_2/image', Image, self.object_commander.ros_raw_gripper_2_image_cb)
self.ros_gripper_2_trigger = rospy.ServiceProxy('/gripper_camera_2/trigger', Trigger)
# --- Camera parameters
self.CamParam = CameraParams()
# --- Camera pose
#TODO: Substitute transform in here
R_Jcam = np.array([[0.9995,-0.0187,-0.0263],[-0.0191,-0.9997,-0.0135],[-0.0261,0.0140,-0.9996]])
r_cam = rox.hat(np.array([0.0707, 1.1395, 0.2747]))#rox.hat(np.array([- 0.2811, -1.1397,0.0335]))#rox.hat(np.array([- 0.2811, 1.1397,0.0335]))
self.R_Jcam = np.linalg.inv(np.vstack([ np.hstack([R_Jcam,np.zeros([3,3])]), np.hstack([np.dot(r_cam,R_Jcam),R_Jcam]) ]))
self.dt = None
self.iteration=0
self.board_ground = None
# functions like a gain, used with velocity to track position
self.FTdata = None
self.ft_flag = False
#self.FTdata_0 = self.FTdata
#self.FTdata_0est = self.compute_ft_est()
#self.result = self.take_image()
#TODO: replace with trajopt code
self.client = actionlib.SimpleActionClient("joint_trajectory_action", FollowJointTrajectoryAction)
# IBVS parameters
self.du_converge_TH = None
self.dv_converge_TH = None
self.iteration_limit = None
self.Ki = None
# Compliance controller parameters
self.F_d_set1 = None
self.F_d_set2 = None
self.Kc = None
self.ros_data=None
self.camMatrix=None
self.distCoeffs=None
self.ros_gripper_2_cam_info_sub=rospy.Subscriber('/gripper_camera_2/camera_info', CameraInfo, self.fill_camera_data)
def __init__(self, disable_ft=False):
self.lock = threading.Lock()
urdf_xml_string = rospy.get_param("robot_description")
srdf_xml_string = rospy.get_param("robot_description_semantic")
self.urdf = URDF.from_parameter_server()
self.overhead_vision_client = actionlib.SimpleActionClient(
"recognize_objects", ObjectRecognitionAction)
self.rapid_node = rapid_node_pkg.RAPIDCommander()
self.controller_commander = ControllerCommander()
self.state = 'init'
self.current_target = None
self.current_payload = None
self.available_payloads = {
'leeward_mid_panel': 'leeward_mid_panel',
'leeward_tip_panel': 'leeward_tip_panel'
}
self.desired_controller_mode = self.controller_commander.MODE_AUTO_TRAJECTORY
self.speed_scalar = 1.0
self.disable_ft = disable_ft
self.tf_listener = PayloadTransformListener()
self._process_state_pub = rospy.Publisher("process_state",
ProcessState,
queue_size=100,
latch=True)
self.publish_process_state()
self.placement_client = actionlib.SimpleActionClient(
'placement_step', PBVSPlacementAction)
#self.placement_client.wait_for_server()
self.update_payload_pose_srv = rospy.ServiceProxy(
"update_payload_pose", UpdatePayloadPose)
self.get_payload_array_srv = rospy.ServiceProxy(
"get_payload_array", GetPayloadArray)
self._goal_handle = None
self._goal_handle_lock = threading.RLock()
self.subprocess_handle = None
self.plan_dictionary = {}
self.process_starts = {}
self.process_index = None
self.process_states = [
"reset_position", "pickup_prepare", "pickup_lower",
"pickup_grab_first_step", "pickup_grab_second_step",
"pickup_raise", "transport_payload", "place_payload",
"gripper_release"
]
self.planner = Planner(self.controller_commander, urdf_xml_string,
srdf_xml_string)
class ObjectRecognitionCommander(object):
def __init__(self):
self.client = actionlib.SimpleActionClient("recognize_objects",
ObjectRecognitionAction)
self.listener = PayloadTransformListener()
def get_object_pose(self, key):
self.client.wait_for_server()
goal = ObjectRecognitionGoal(False, [-1e10, 1e10, -1e10, 1e10])
self.client.send_goal(goal)
self.client.wait_for_result()
ret = self.client.get_result()
for r in ret.recognized_objects.objects:
if r.type.key == key:
return rox_msg.msg2transform(r.pose.pose.pose)
raise Exception("Requested object not found")
def get_object_gripper_target_pose(self, key):
object_pose = self.get_object_pose(key)
tag_rel_pose = self.listener.lookupTransform(key,
key + "_gripper_target",
rospy.Time(0))
return object_pose * tag_rel_pose
def main():
rospy.init_node("pbvs_object_placement")
urdf_xml_string = rospy.get_param("robot_description")
srdf_xml_string = rospy.get_param("robot_description_semantic")
controller_commander = ControllerCommander()
#planner = Planner(controller_commander, urdf_xml_string, srdf_xml_string)
transform_fname = sys.argv[1]
camera_image_topic = sys.argv[2]
camera_trigger_topic = sys.argv[3]
camera_info_topic = sys.argv[4]
tf_listener = PayloadTransformListener()
desired_transform_msg = TransformStamped()
with open(transform_fname, 'r') as f:
transform_yaml = yaml.load(f)
genpy.message.fill_message_args(desired_transform_msg, transform_yaml)
desired_transform = rox_msg.msg2transform(desired_transform_msg)
markers = get_all_payload_markers()
fixed_marker = markers[desired_transform.parent_frame_id]
payload_marker = markers[desired_transform.child_frame_id]
aruco_dict = get_aruco_dictionary(fixed_marker)
camera_info = get_camera_info(camera_info_topic)
time.sleep(1)
dx = np.array([10000] * 6)
while True:
img = receive_image(camera_image_topic, camera_trigger_topic)
target_pose = compute_step_gripper_target_pose(img, fixed_marker, payload_marker, desired_transform, \
camera_info, aruco_dict, np.array([0.2]*6), tf_listener)
plan = planner.trajopt_plan(target_pose,
json_config_name="panel_pickup")
controller_commander.set_controller_mode(
controller_commander.MODE_HALT, 1, [], [])
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 1, [], [])
controller_commander.execute_trajectory(plan)
class TestTesseractDiff(unittest.TestCase):
def __init__(self, *args, **kwargs):
super(TestTesseractDiff, self).__init__(*args, **kwargs)
self.tf_listener = PayloadTransformListener()
def test_tesseract_diff(self):
expected_state_1 = rospy.wait_for_message("/expected_tesseract_diff_1",
TesseractState,
timeout=10)
state_1 = rospy.wait_for_message('/tesseract_diff',
TesseractState,
timeout=10)
self._assert_tesseract_state_equal(expected_state_1, state_1)
rospy.sleep(rospy.Duration(1))
update_pose_proxy = rospy.ServiceProxy("update_payload_pose",
UpdatePayloadPose)
payload2_to_gripper_target_tf = self.tf_listener.lookupTransform(
"payload2", "payload2_gripper_target", rospy.Time(0))
req = UpdatePayloadPoseRequest()
req.header.frame_id = "vacuum_gripper_tool"
req.name = "payload2"
req.pose = rox_msg.transform2pose_msg(
payload2_to_gripper_target_tf.inv())
req.confidence = 0.5
res = update_pose_proxy(req)
assert res.success
expected_state_2 = rospy.wait_for_message("/expected_tesseract_diff_2",
TesseractState,
timeout=10)
state_2 = rospy.wait_for_message('/tesseract_diff',
TesseractState,
timeout=10)
self._assert_tesseract_state_equal(expected_state_2, state_2)
world_to_fixture2_payload2_target_tf = self.tf_listener.lookupTransform(
"world", "fixture2_payload2_target", rospy.Time(0))
#Add in an offset to represent a final placement error
fixture2_payload2_target_to_payload2_tf = rox.Transform(
rox.rot([0, 0, 1], np.deg2rad(5)), [0.1, 0, 0],
"fixture2_payload2_target", "payload2")
req2 = UpdatePayloadPoseRequest()
req2.header.frame_id = "world"
req2.name = "payload2"
req2.pose = rox_msg.transform2pose_msg(
world_to_fixture2_payload2_target_tf *
fixture2_payload2_target_to_payload2_tf)
res2 = update_pose_proxy(req2)
assert res2.success
expected_state_3 = rospy.wait_for_message("/expected_tesseract_diff_3",
TesseractState,
timeout=10)
state_3 = rospy.wait_for_message('/tesseract_diff',
TesseractState,
timeout=10)
self._assert_tesseract_state_equal(expected_state_3, state_3)
def _assert_tesseract_state_equal(self, state1, state2):
self.assertEqual(state1.name, state2.name)
self.assertEqual(state1.urdf_name, state2.urdf_name)
self.assertEqual(state1.object_colors, state2.object_colors)
self.assertEqual(state1.highlight_links, state2.object_colors)
self.assertEqual(state1.is_diff, state2.is_diff)
self.assertEqual(len(state1.attachable_objects),
len(state2.attachable_objects))
for i in xrange(len(state1.attachable_objects)):
self._assert_attachable_object_equal(state1.attachable_objects[i],
state2.attachable_objects[i])
self.assertEqual(len(state1.attached_bodies),
len(state2.attached_bodies))
for i in xrange(len(state1.attached_bodies)):
self._assert_attached_body_equal(state1.attached_bodies[i],
state2.attached_bodies[i])
def _assert_attachable_object_equal(self, obj1, obj2):
self.assertEqual(obj1.name, obj2.name)
self._assert_visual_geometry_equal(obj1.visual, obj2.visual)
self._assert_collision_geometry_equal(obj1.collision, obj2.collision)
self._assert_inertia_almost_equal(obj1.inertia, obj2.inertia)
self.assertEqual(obj1.operation, obj2.operation)
def _assert_visual_geometry_equal(self, vis1, vis2):
self.assertEqual(vis1.primitives, vis2.primitives)
self._assert_pose_almost_equal(vis1.primitive_poses,
vis2.primitive_poses)
self.assertEqual(vis1.primitive_colors, vis2.primitive_colors)
for m1, m2 in zip(vis1.meshes, vis2.meshes):
self._assert_mesh_almost_equal(m1, m2)
self._assert_pose_almost_equal(vis1.mesh_poses, vis2.mesh_poses)
self.assertEqual(vis1.mesh_colors, vis2.mesh_colors)
self.assertEqual(vis1.planes, vis2.planes)
self._assert_pose_almost_equal(vis1.plane_poses, vis2.plane_poses)
self.assertEqual(vis1.plane_colors, vis2.plane_colors)
self.assertEqual(vis1.octomaps, vis2.octomaps)
self._assert_pose_almost_equal(vis1.octomap_poses, vis2.octomap_poses)
self.assertEqual(vis1.octomap_colors, vis2.octomap_colors)
def _assert_collision_geometry_equal(self, col1, col2):
self.assertEqual(col1.primitives, col2.primitives)
self._assert_pose_almost_equal(col1.primitive_poses,
col2.primitive_poses)
self.assertEqual(col1.primitive_collision_object_types,
col2.primitive_collision_object_types)
self.assertEqual(col1.primitive_colors, col2.primitive_colors)
for m1, m2 in zip(col1.meshes, col2.meshes):
self._assert_mesh_almost_equal(m1, m2)
self._assert_pose_almost_equal(col1.mesh_poses, col2.mesh_poses)
self.assertEqual(col1.mesh_collision_object_types,
col2.mesh_collision_object_types)
self.assertEqual(col1.mesh_colors, col2.mesh_colors)
self.assertEqual(col1.planes, col2.planes)
self._assert_pose_almost_equal(col1.plane_poses, col2.plane_poses)
self.assertEqual(col1.plane_collision_object_types,
col2.plane_collision_object_types)
self.assertEqual(col1.plane_colors, col2.plane_colors)
self.assertEqual(col1.octomaps, col2.octomaps)
self._assert_pose_almost_equal(col1.octomap_poses, col2.octomap_poses)
self.assertEqual(col1.octomap_collision_object_types,
col2.octomap_collision_object_types)
self.assertEqual(col1.octomap_colors, col2.octomap_colors)
def _assert_attached_body_equal(self, body1, body2):
self.assertEqual(body1.object_name, body2.object_name)
self.assertEqual(body1.parent_link_name, body2.parent_link_name)
self._assert_pose_almost_equal(body1.transform, body2.transform)
self.assertEqual(body1.touch_links, body2.touch_links)
self.assertEqual(body1.operation, body2.operation)
def _assert_mesh_almost_equal(self, m1, m2):
for t1, t2 in zip(m1.triangles, m2.triangles):
np.testing.assert_equal(t1.vertex_indices, t2.vertex_indices)
for v1, v2 in zip(m1.vertices, m2.vertices):
np.testing.assert_allclose([v1.x, v1.y, v1.z], [v2.x, v2.y, v2.z])
def _assert_pose_almost_equal(self, pose1, pose2):
if isinstance(pose1, list):
for p1, p2 in zip(pose1, pose2):
self._assert_pose_almost_equal(p1, p2)
return
np.testing.assert_allclose([pose1.position.x, pose1.position.y, pose1.position.z], \
[pose2.position.x, pose2.position.y, pose2.position.z])
np.testing.assert_allclose([pose1.orientation.x, pose1.orientation.y, \
pose1.orientation.z, pose1.orientation.w], \
[pose2.orientation.x, pose2.orientation.y, \
pose2.orientation.z, pose2.orientation.w] )
def _assert_inertia_almost_equal(self, i1, i2):
self.assertAlmostEqual(i1.m, i2.m)
self.assertAlmostEqual(i1.com.x, i2.com.x)
self.assertAlmostEqual(i1.com.y, i2.com.y)
self.assertAlmostEqual(i1.com.z, i2.com.z)
self.assertAlmostEqual(i1.ixx, i2.ixx)
self.assertAlmostEqual(i1.ixy, i2.ixy)
self.assertAlmostEqual(i1.ixz, i2.ixz)
self.assertAlmostEqual(i1.ixy, i2.ixy)
self.assertAlmostEqual(i1.iyy, i2.iyy)
self.assertAlmostEqual(i1.iyz, i2.iyz)
self.assertAlmostEqual(i1.izz, i2.izz)
class ProcessController(object):
def __init__(self, disable_ft=False):
self.urdf = URDF.from_parameter_server()
self.overhead_vision_client = actionlib.SimpleActionClient(
"recognize_objects", ObjectRecognitionAction)
self.execute_trajectory_action = actionlib.SimpleActionClient(
"execute_trajectory", ExecuteTrajectoryAction)
self.rapid_node = rapid_node_pkg.RAPIDCommander()
self.controller_commander = controller_commander_pkg.ControllerCommander(
)
self.state = 'init'
self.current_target = None
self.current_payload = None
self.available_payloads = {
'leeward_mid_panel': 'leeward_mid_panel',
'leeward_tip_panel': 'leeward_tip_panel'
}
self.desired_controller_mode = self.controller_commander.MODE_AUTO_TRAJECTORY
self.speed_scalar = 1.0
self.disable_ft = disable_ft
self.tf_listener = PayloadTransformListener()
self._process_state_pub = rospy.Publisher("process_state",
ProcessState,
queue_size=100,
latch=True)
self.publish_process_state()
self.update_payload_pose_srv = rospy.ServiceProxy(
"update_payload_pose", UpdatePayloadPose)
self.get_payload_array_srv = rospy.ServiceProxy(
"get_payload_array", GetPayloadArray)
self.plan_dictionary = {}
def _vision_get_object_pose(self, key):
self.overhead_vision_client.wait_for_server()
goal = ObjectRecognitionGoal(False, [-1e10, 1e10, -1e10, 1e10])
self.overhead_vision_client.send_goal(goal)
self.overhead_vision_client.wait_for_result()
ret = self.overhead_vision_client.get_result()
for r in ret.recognized_objects.objects:
if r.type.key == key:
rox_pose = rox_msg.msg2transform(r.pose.pose.pose)
rox_pose.parent_frame_id = r.pose.header.frame_id
rox_pose.child_frame_id = key
return rox_pose
raise Exception("Requested object not found")
def _vision_get_object_gripper_target_pose(self, key):
object_pose = self._vision_get_object_pose(key)
tag_rel_pose = self.tf_listener.lookupTransform(
key, key + "_gripper_target", rospy.Time(0))
return object_pose * tag_rel_pose, object_pose
def _tf_get_object_gripper_target_pose(self, key):
payload = self._get_payload(key)
if payload.confidence < 0.8:
raise Exception("Payload confidence too low for tf lookup")
object_pose = self.tf_listener.lookupTransform("/world", key,
rospy.Time(0))
tag_rel_pose = self.tf_listener.lookupTransform(
key, key + "_gripper_target", rospy.Time(0))
return object_pose * tag_rel_pose, object_pose
def get_payload_pickup_ft_threshold(self, payload):
if self.disable_ft:
return []
return self._get_payload(
payload).gripper_targets[0].pickup_ft_threshold
def _active_client(self):
self.state = "moving"
#self.publish_process_state()
def _finished_client(self, state, result):
#if(state== actionlib.GoalStatus.SUCCEEDED):
self.publish_process_state()
if (result != MoveItErrorCodes.SUCCESS):
rospy.loginfo(MoveItErrorCodes.SUCCESS)
rospy.loginfo("MoveItErrorCode generated: %s", str(result))
def get_state(self):
return self.state
def get_current_pose(self):
return self.controller_commander.get_current_pose()
def plan_to_reset_position(self):
#TODO: Implement reset movement in process controller
rospy.loginfo("Planning to reset position")
self.state = "reset_position"
self.plan_dictionary['reset'] = path
self.publish_process_state()
def plan_pickup_prepare(self, target_payload):
#TODO: check state and payload
rospy.loginfo("Begin pickup_prepare for payload %s", target_payload)
object_target, object_pose = self._vision_get_object_gripper_target_pose(
target_payload)
self._update_payload_pose(target_payload,
object_pose,
confidence=0.8,
parent_frame_id=object_pose.parent_frame_id)
rospy.loginfo("Found payload %s at pose %s", target_payload,
object_target)
self.pose_target = copy.deepcopy(object_target)
pose_target = self.pose_target
pose_target.p[2] += 0.5
rospy.loginfo("Prepare pickup %s at pose %s", target_payload,
object_target)
print pose_target.p
path = self.controller_commander.plan(pose_target)
self.current_target = target_payload
self.state = "plan_pickup_prepare"
self.plan_dictionary['pickup_prepare'] = path
rospy.loginfo("Finish pickup prepare for payload %s", target_payload)
self.publish_process_state()
def move_pickup_prepare(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, self.speed_scalar, [], [])
result = None
self.state = "pickup_prepare"
goal = ExecuteTrajectoryGoal()
goal.trajectory = self.plan_dictionary['pickup_prepare']
self.execute_trajectory_action.send_goal(goal,
active_cb=self._active_client,
done_cb=self._finished_client)
#self.controller_commander.async_execute(self.plan_dictionary['pickup_prepare'],result)
def plan_pickup_lower(self):
#TODO: check change state and target
rospy.loginfo("Begin pickup_lower for payload %s", self.current_target)
object_target, _ = self._tf_get_object_gripper_target_pose(
self.current_target)
pose_target2 = copy.deepcopy(object_target)
pose_target2.p[2] += 0.15
print pose_target2.p
path = self.controller_commander.compute_cartesian_path(
pose_target2, avoid_collisions=False)
self.state = "plan_pickup_lower"
self.plan_dictionary['pickup_lower'] = path
rospy.loginfo("Finish pickup_lower for payload %s",
self.current_target)
self.publish_process_state()
def move_pickup_lower(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.8 * self.speed_scalar, [],
self.get_payload_pickup_ft_threshold(self.current_target))
result = None
self.state = "pickup_lower"
goal = ExecuteTrajectoryGoal()
goal.trajectory = self.plan_dictionary['pickup_lower']
self.execute_trajectory_action.send_goal(goal,
active_cb=self._active_client,
done_cb=self._finished_client)
#self.controller_commander.execute(self.plan_dictionary['pickup_lower'])
def plan_pickup_grab_first_step(self):
#TODO: check change state and target
rospy.loginfo("Begin pickup_grab for payload %s", self.current_target)
self.object_target, _ = self._tf_get_object_gripper_target_pose(
self.current_target)
pose_target2 = copy.deepcopy(self.object_target)
pose_target2.p[2] -= 0.15
path = self.controller_commander.compute_cartesian_path(
pose_target2, avoid_collisions=False)
self.state = "plan_pickup_grab_first_step"
self.plan_dictionary['pickup_grab_first_step'] = path
self.publish_process_state()
def move_pickup_grab_first_step(self):
self.controller_commander.set_controller_mode(self.desired_controller_mode, 0.4*self.speed_scalar, [],\
self.get_payload_pickup_ft_threshold(self.current_target))
result = None
self.state = "pickup_grab_first_step"
try:
goal = ExecuteTrajectoryGoal()
goal.trajectory = self.plan_dictionary['pickup_grab_first_step']
self.execute_trajectory_action.send_goal(
goal,
active_cb=self._active_client,
done_cb=self._finished_client)
#self.controller_commander.execute(self.plan_dictionary['pickup_grab_first_step'])
except Exception as err:
print err
def plan_pickup_grab_second_step(self):
self.rapid_node.set_digital_io("Vacuum_enable", 1)
time.sleep(1)
#TODO: check vacuum feedback to make sure we have the panel
world_to_panel_tf = self.tf_listener.lookupTransform(
"world", self.current_target, rospy.Time(0))
world_to_gripper_tf = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
panel_to_gripper_tf = world_to_gripper_tf.inv() * world_to_panel_tf
self.current_payload = self.current_target
self.current_target = None
self._update_payload_pose(self.current_payload, panel_to_gripper_tf,
"vacuum_gripper_tool", 0.5)
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, 1, [], [])
time.sleep(1)
pose_target2 = copy.deepcopy(self.object_target)
pose_target2.p[2] += 0.15
path = self.controller_commander.compute_cartesian_path(
pose_target2, avoid_collisions=False)
self.state = "plan_pickup_grab_second_step"
self.plan_dictionary['pickup_grab_second_step'] = path
rospy.loginfo("Finish pickup_grab for payload %s", self.current_target)
self.publish_process_state()
def move_pickup_grab_second_step(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.4 * self.speed_scalar, [], [])
result = None
self.state = "pickup_grab_second_step"
goal = ExecuteTrajectoryGoal()
goal.trajectory = self.plan_dictionary['pickup_grab_second_step']
self.execute_trajectory_action.send_goal(goal,
active_cb=self._active_client,
done_cb=self._finished_client)
#self.controller_commander.execute(self.plan_dictionary['pickup_grab_second_step'])
def plan_pickup_raise(self):
#TODO: check change state and target
rospy.loginfo("Begin pickup_raise for payload %s",
self.current_payload)
#Just use gripper position for now, think up a better way in future
object_target = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
pose_target2 = copy.deepcopy(object_target)
pose_target2.p[2] += 0.8
pose_target2.p = np.array([-0.02285, -1.840, 1.0])
pose_target2.R = rox.q2R([0.0, 0.707, 0.707, 0.0])
path = self.controller_commander.compute_cartesian_path(
pose_target2, avoid_collisions=False)
self.state = "plan_pickup_raise"
self.plan_dictionary['pickup_raise'] = path
rospy.loginfo("Finish pickup_raise for payload %s",
self.current_target)
self.publish_process_state()
def move_pickup_raise(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.8 * self.speed_scalar, [], [])
result = None
self.state = "pickup_raise"
goal = ExecuteTrajectoryGoal()
goal.trajectory = self.plan_dictionary['pickup_raise']
self.execute_trajectory_action.send_goal(goal,
active_cb=self._active_client,
done_cb=self._finished_client)
#self.controller_commander.async_execute(self.plan_dictionary['pickup_raise'],result)
def plan_transport_payload(self, target):
#TODO: check state and payload
rospy.loginfo("Begin transport_panel for payload %s to %s",
self.current_payload, target)
panel_target_pose = self.tf_listener.lookupTransform(
"world", target, rospy.Time(0))
panel_gripper_pose = self.tf_listener.lookupTransform(
self.current_payload, "vacuum_gripper_tool", rospy.Time(0))
pose_target = panel_target_pose * panel_gripper_pose
pose_target.p = [
2.197026484647054, 1.2179574262842452, 0.12376598588449844
]
pose_target.R = np.array([[-0.99804142, 0.00642963, 0.06222524],
[0.00583933, 0.99993626, -0.00966372],
[-0.06228341, -0.00928144, -0.99801535]])
pose_target.p[2] += 0.35
plan = self.controller_commander.plan(pose_target)
self.current_target = target
self.state = "plan_transport_panel"
self.plan_dictionary['transport_payload'] = plan
rospy.loginfo("Finish transport_panel for payload %s to %s",
self.current_payload, target)
self.publish_process_state()
def move_transport_payload(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.8 * self.speed_scalar, [], [])
result = None
self.state = "transport_panel"
self.controller_commander.async_execute(
self.plan_dictionary['transport_payload'], result)
self.publish_process_state()
def plan_place_lower(self):
#TODO: check state and payload
rospy.loginfo("Begin place_lower for payload %s to %s",
self.current_payload, self.current_target)
panel_target_pose = self.tf_listener.lookupTransform(
"world", self.current_target, rospy.Time(0))
panel_gripper_pose = self.tf_listener.lookupTransform(
self.current_payload, "vacuum_gripper_tool", rospy.Time(0))
pose_target = panel_target_pose * panel_gripper_pose
pose_target.p[2] += 0.15
path = self.controller_commander.plan(pose_target)
self.state = "plan_place_lower"
self.plan_dictionary['place_lower'] = path
rospy.loginfo("Finish place_lower for payload %s to %s",
self.current_payload, self.current_target)
self.publish_process_state()
def move_place_lower(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.8 * self.speed_scalar, [], [])
result = None
self.state = "place_lower"
self.controller_commander.async_execute(
self.plan_dictionary['place_lower'], result)
self.publish_process_state()
def plan_place_set_first_step(self):
#TODO: check change state and target
rospy.loginfo("Begin place_set for payload %s target %s",
self.current_payload, self.current_target)
panel_target_pose = self.tf_listener.lookupTransform(
"world", self.current_target, rospy.Time(0))
panel_gripper_pose = self.tf_listener.lookupTransform(
self.current_payload, "vacuum_gripper_tool", rospy.Time(0))
pose_target = panel_target_pose * panel_gripper_pose
pose_target2 = copy.deepcopy(pose_target)
pose_target2.p[2] -= 0.15
path = self.controller_commander.compute_cartesian_path(
pose_target2, avoid_collisions=False)
self.state = "plan_place_set_first_step"
self.plan_dictionary['place_set_first_step'] = path
self.publish_process_state()
def move_place_set_first_step(self):
self.controller_commander.set_controller_mode(self.desired_controller_mode, 0.4*self.speed_scalar, [], \
self.get_payload_pickup_ft_threshold(self.current_target))
result = None
self.state = "place_set_first_step"
self.controller_commander.async_execute(
self.plan_dictionary['place_set_first_step'], result)
self.rapid_node.set_digital_io("Vacuum_enable", 0)
time.sleep(1)
self.publish_process_state()
def plan_place_set_second_step(self):
#TODO: check vacuum feedback to make sure we have the panel
gripper_to_panel_tf = self.tf_listener.lookupTransform(
"vacuum_gripper_tool", self.current_payload, rospy.Time(0))
world_to_gripper_tf = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
world_to_panel_nest_tf = self.tf_listener.lookupTransform(
"world", "panel_nest", rospy.Time(0))
panel_to_nest_tf = world_to_panel_nest_tf.inv(
) * world_to_gripper_tf * gripper_to_panel_tf
self._update_payload_pose(self.current_payload, panel_to_nest_tf,
"panel_nest", 0.5)
self.current_payload = None
self.current_target = None
'''
time.sleep(1)
pose_target2=copy.deepcopy(pose_target)
pose_target2.p[2] += 0.15
path=self.controller_commander.compute_cartesian_path(pose_target2, avoid_collisions=False)
self.plan_dictionary['place_set_second_step']=path
rospy.loginfo("Finish place_set for payload %s", self.current_target)
'''
self.state = "plan_place_set_second_step"
self.publish_process_state()
def move_place_set_second_step(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.4 * self.speed_scalar, [], [])
result = None
self.state = "place_set_second_step"
self.controller_commander.async_execute(
self.plan_dictionary['place_set_second_step'], result)
self.publish_process_state()
def plan_place_raise(self):
#TODO: check change state and target
rospy.loginfo("Begin place_raise for payload %s", self.current_payload)
#Just use gripper position for now, think up a better way in future
object_target = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
pose_target2 = copy.deepcopy(object_target)
pose_target2.p[2] += 0.5
path = self.controller_commander.compute_cartesian_path(
pose_target2, avoid_collisions=False)
self.state = "plan_place_raise"
self.plan_dictionary['place_raise'] = path
rospy.loginfo("Finish place_raise for payload %s", self.current_target)
self.publish_process_state()
def move_place_raise(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.8 * self.speed_scalar, [], [])
result = None
self.state = "place_raise"
self.controller_commander.async_execute(
self.plan_dictionary['place_raise'], result)
self.publish_process_state()
def _fill_process_state(self):
s = ProcessState()
s.state = self.state if self.state is not None else ""
s.payload = self.current_payload if self.current_payload is not None else ""
s.target = self.current_target if self.current_target is not None else ""
return s
def publish_process_state(self):
s = self._fill_process_state()
self._process_state_pub.publish(s)
def place_lower_temp(self):
UV = np.zeros([32, 2])
P = np.zeros([32, 3])
def _update_payload_pose(self,
payload_name,
pose,
parent_frame_id=None,
confidence=0.1):
payload = self._get_payload(payload_name)
if parent_frame_id is None:
parent_frame_id = payload.header.frame_id
parent_tf = self.tf_listener.lookupTransform(parent_frame_id,
pose.parent_frame_id,
rospy.Time(0))
pose2 = parent_tf.inv() * pose
req = UpdatePayloadPoseRequest()
req.name = payload_name
req.pose = rox_msg.transform2pose_msg(pose2)
req.header.frame_id = parent_frame_id
req.confidence = confidence
res = self.update_payload_pose_srv(req)
if not res.success:
raise Exception("Could not update payload pose")
def _get_payload(self, payload_name):
payload_array_res = self.get_payload_array_srv(
GetPayloadArrayRequest([payload_name]))
if len(payload_array_res.payload_array.payloads
) != 1 or payload_array_res.payload_array.payloads[
0].name != payload_name:
raise Exception("Invalid payload specified")
return payload_array_res.payload_array.payloads[0]
class SimulatedVisionServer(object):
def ros_image_cb(self, ros_data):
np_arr = np.fromstring(ros_data.data, np.uint8)
ros_image1 = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
if len(ros_image1) > 2 and ros_image1.shape[2] == 4:
self.ros_image = cv2.cvtColor(ros_image1, cv2.COLOR_BGRA2BGR)
else:
self.ros_image = ros_image1
self.ros_image_stamp = ros_data.header.stamp
def ros_raw_overhead_image_cb(self, ros_data):
self.ros_overhead_cam_info.take_image(ros_data)
self.ros_image_stamp = ros_data.header.stamp
def ros_raw_gripper_1_image_cb(self, ros_data):
self.ros_gripper_1_cam_info.take_image(ros_data)
self.ros_image_stamp = ros_data.header.stamp
def ros_raw_gripper_2_image_cb(self, ros_data):
self.ros_gripper_2_cam_info.take_image(ros_data)
self.ros_image_stamp = ros_data.header.stamp
def ros_cam_info_cb(self, ros_data):
if (ros_data.header.frame_id == "overhead_camera"):
self.ros_overhead_cam_info.fill_data(ros_data)
if (ros_data.header.frame_id == "gripper_camera_1"):
self.ros_gripper_1_cam_info.fill_data(ros_data)
if (ros_data.header.frame_id == "gripper_camera_2"):
self.ros_gripper_2_cam_info.fill_data(ros_data)
# TODO: handle different action namespaces, camera namespaces, etc
def __init__(self,
frame_id="world",
action_ns_1="recognize_objects",
action_ns_2="recognize_objects_gripper"):
self.bridge = CvBridge()
self.overhead_server = SimpleActionServer(
action_ns_1,
ObjectRecognitionAction,
execute_cb=self.execute_overhead_callback)
self.gripper_server = SimpleActionServer(
action_ns_2,
ObjectRecognitionAction,
execute_cb=self.execute_gripper_callback)
self.recognized_objects = dict()
self.listener = PayloadTransformListener()
self.frame_id = "world"
self.ros_image = None
self.ros_image_stamp = rospy.Time(0)
self.last_ros_image_stamp = rospy.Time(0)
self.ros_overhead_cam_info = CameraInfoStorage()
self.ros_gripper_1_cam_info = CameraInfoStorage()
self.ros_gripper_2_cam_info = CameraInfoStorage()
#TODO: Handle compressed images vs uncompressed images better. Note that
#the uncompressed 20 MP color images don't seem to be received properly.
if not "compressed-image" in sys.argv:
self.ros_overhead_img_sub = rospy.Subscriber(
'/overhead_camera/image', Image,
self.ros_raw_overhead_image_cb)
self.ros_gripper_1_img_sub = rospy.Subscriber(
'/gripper_camera_1/image', Image,
self.ros_raw_gripper_1_image_cb)
self.ros_gripper_2_img_sub = rospy.Subscriber(
'/gripper_camera_2/image', Image,
self.ros_raw_gripper_2_image_cb)
else:
self.ros_overhead_img_sub = rospy.Subscriber(
'/overhead_camera/image/compressed', CompressedImage,
self.ros_image_cb)
self.ros_gripper_1_img_sub = rospy.Subscriber(
'/gripper_camera_1/image/compressed', CompressedImage,
self.ros_image_cb)
self.ros_gripper_2_img_sub = rospy.Subscriber(
'/gripper_camera_2/image/compressed', CompressedImage,
self.ros_image_cb)
self.ros_overhead_trigger = rospy.ServiceProxy(
'/overhead_camera/trigger', Trigger)
self.ros_overhead_cam_info_sub = rospy.Subscriber(
'/overhead_camera/camera_info', CameraInfo, self.ros_cam_info_cb)
self.ros_gripper_1_trigger = rospy.ServiceProxy(
'/gripper_camera_1/trigger', Trigger)
self.ros_gripper_2_trigger = rospy.ServiceProxy(
'/gripper_camera_2/trigger', Trigger)
self.ros_gripper_1_cam_info_sub = rospy.Subscriber(
'/gripper_camera_1/camera_info', CameraInfo, self.ros_cam_info_cb)
self.ros_gripper_2_cam_info_sub = rospy.Subscriber(
'/gripper_camera_2/camera_info', CameraInfo, self.ros_cam_info_cb)
#self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
self.payloads = dict()
self.link_markers = dict()
self.payloads_lock = threading.Lock()
self.payloads_sub = rospy.Subscriber("payload", PayloadArray,
self._payload_msg_cb)
def _payload_msg_cb(self, msg):
with self.payloads_lock:
for p in msg.payloads:
if p.name not in self.payloads:
self.payloads[p.name] = p
else:
payload = self.payloads[p.name]
#ignore stale data
if payload.header.stamp > p.header.stamp:
continue
self.payloads[p.name] = p
for l in msg.link_markers:
if l.header.frame_id not in self.link_markers:
self.link_markers[l.header.frame_id] = l
else:
ll = self.link_markers[l.header.frame_id]
#ignore stale data
if ll.header.stamp > l.header.stamp:
continue
self.link_markers[l.header.frame_id] = l
for d in msg.delete_payloads:
if d in self.payloads:
del self.payloads[d]
if d in self.link_markers:
del self.link_markers[d]
def execute_overhead_callback(self, goal):
now = rospy.Time.now()
if self.ros_overhead_cam_info is None:
raise Exception("Camera Info not received")
self.last_ros_image_stamp = self.ros_image_stamp
try:
self.ros_overhead_trigger.wait_for_service(timeout=0.1)
self.ros_overhead_trigger()
except:
pass
wait_count = 0
while self.ros_overhead_cam_info.ros_image is None or self.ros_image_stamp == self.last_ros_image_stamp:
if wait_count > 250:
raise Exception("Image receive timeout")
time.sleep(0.25)
wait_count += 1
print "Past timeout"
#stored data in local function variable, so changed all references off of self.ros_image
img = self.ros_overhead_cam_info.ros_image
if img is None:
raise Exception("Camera image data not received")
print "image received"
r_array = RecognizedObjectArray()
r_array.header.stamp = now
r_array.header.frame_id = self.frame_id
if goal.use_roi:
raise Warning("use_roi in ObjectRecognitionRequest ignored")
search_objects = dict()
with self.payloads_lock:
for _, p in self.payloads.items():
search_objects[p.name] = p.markers
for _, l in self.link_markers.items():
search_objects[l.header.frame_id] = l.markers
#TODO: handle multiple aruco dictionaries, currently only use one
print "past payload lock"
aruco_dicts = set()
for m in search_objects.itervalues():
for m2 in m:
aruco_dicts.add(m2.marker.dictionary)
print len(aruco_dicts)
aruco_dicts.add("DICT_ARUCO_ORIGINAL")
assert len(
aruco_dicts
) == 1, "Currently all tags must be from the same dictionary"
if not hasattr(cv2.aruco, next(iter(aruco_dicts))):
raise ValueError("Invalid aruco-dict value")
aruco_dict_id = getattr(cv2.aruco, next(iter(aruco_dicts)))
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id)
c_pose = self.listener.lookupTransform(
"/world", self.ros_overhead_cam_info.ros_data.header.frame_id,
rospy.Time(0))
print "c_pose"
print c_pose.p
print c_pose.R
parameters = cv2.aruco.DetectorParameters_create()
parameters.cornerRefinementWinSize = 32
parameters.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
img, aruco_dict, parameters=parameters)
print ids
for object_name, payload_markers in search_objects.items():
tag_object_poses = dict()
for m in payload_markers:
board = get_aruco_gridboard(m.marker, aruco_dict)
#board = cv2.aruco.GridBoard_create(4, 4, .04, .0075, aruco_dict, 16)
retval, rvec, tvec = cv2.aruco.estimatePoseBoard(
corners, ids, board, self.ros_overhead_cam_info.camMatrix,
self.ros_overhead_cam_info.distCoeffs)
print retval
print rvec
print tvec
if (retval > 0):
if (m.name == "leeward_mid_panel_marker_1"
or m.name == "leeward_tip_panel_marker_1"):
print "Found tag: " + m.name
try:
#o_pose = self.listener.lookupTransform(m.name, object_name, rospy.Time(0))
o_pose = rox_msg.msg2transform(m.pose).inv()
print object_name
print o_pose
print ""
print "o_pose"
print o_pose.p
print o_pose.R
Ra, b = cv2.Rodrigues(rvec)
a_pose = rox.Transform(Ra, tvec)
print "a_pose"
print a_pose.p
print a_pose.R
tag_object_poses[m.name] = c_pose * (a_pose *
o_pose)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
#TODO: merge tag data if multiple tags detected
if len(tag_object_poses) > 0:
p = PoseWithCovarianceStamped()
p.pose.pose = rox_msg.transform2pose_msg(
tag_object_poses.itervalues().next())
p.header.stamp = now
p.header.frame_id = self.frame_id
r = RecognizedObject()
r.header.stamp = now
r.header.frame_id = self.frame_id
r.type.key = object_name
r.confidence = 1
r.pose = p
r_array.objects.append(r)
"""for o in self.object_names:
try:
(trans,rot) = self.listener.lookupTransform("/world", o, rospy.Time(0))
print o
print trans
print rot
print ""
p=PoseWithCovarianceStamped()
p.pose.pose=Pose(Point(trans[0], trans[1], trans[2]), Quaternion(rot[0], rot[1], rot[2], rot[3]))
p.header.stamp=now
p.header.frame_id=self.frame_id
r=RecognizedObject()
r.header.stamp=now
r.header.frame_id=self.frame_id
r.type.key=o
r.confidence=1
r.pose=p
r_array.objects.append(r)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue"""
print "Succeeded in finding tags"
result = ObjectRecognitionResult()
result.recognized_objects = r_array
self.overhead_server.set_succeeded(result=result)
def execute_gripper_callback(self, goal):
now = rospy.Time.now()
if self.ros_gripper_1_cam_info is None or self.ros_gripper_2_cam_info is None:
raise Exception("Camera Info not received")
self.last_ros_image_stamp = self.ros_image_stamp
try:
self.ros_gripper_1_trigger.wait_for_service(timeout=0.1)
self.ros_gripper_1_trigger()
self.ros_gripper_2_trigger.wait_for_service(timeout=0.1)
self.ros_gripper_2_trigger()
except:
pass
wait_count = 0
while ((self.ros_gripper_1_cam_info.ros_image is None
and self.ros_gripper_2_cam_info.ros_image is None)
or self.ros_image_stamp == self.last_ros_image_stamp):
if wait_count > 250:
raise Exception("Image receive timeout")
time.sleep(0.25)
wait_count += 1
print "Past timeout"
gripper_1_img = ros_gripper_1_cam_info.ros_image
gripper_2_img = ros_gripper_1_cam_info.ros_image
if gripper_1_img is None or gripper_2_img is None:
raise Exception("Camera image data not received")
print "image received"
#TODO replace code with gripper camera code for placement
'''
r_array=RecognizedObjectArray()
r_array.header.stamp=now
r_array.header.frame_id=self.frame_id
if goal.use_roi:
raise Warning("use_roi in ObjectRecognitionRequest ignored")
search_objects=dict()
with self.payloads_lock:
for _, p in self.payloads.items():
search_objects[p.name] = p.markers
for _, l in self.link_markers.items():
search_objects[l.header.frame_id] = l.markers
#TODO: handle multiple aruco dictionaries, currently only use one
print "past payload lock"
aruco_dicts=set()
for m in search_objects.itervalues():
for m2 in m:
aruco_dicts.add(m2.marker.dictionary)
print len(aruco_dicts)
assert len(aruco_dicts) == 1, "Currently all tags must be from the same dictionary"
if not hasattr(cv2.aruco, next(iter(aruco_dicts))):
raise ValueError("Invalid aruco-dict value")
aruco_dict_id=getattr(cv2.aruco, next(iter(aruco_dicts)))
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id)
c_pose = self.listener.lookupTransform("/world", self.ros_overhead_cam_info.ros_data.header.frame_id, rospy.Time(0))
print "c_pose"
print c_pose.p
print c_pose.R
parameters = cv2.aruco.DetectorParameters_create()
parameters.cornerRefinementWinSize=32
parameters.cornerRefinementMethod=cv2.aruco.CORNER_REFINE_CONTOUR
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(self.ros_image, aruco_dict, parameters=parameters)
for object_name, payload_markers in search_objects.items():
tag_object_poses=dict()
for m in payload_markers:
board = get_aruco_gridboard(m.marker, aruco_dict)
retval, rvec, tvec = cv2.aruco.estimatePoseBoard(corners, ids, board, self.ros_overhead_cam_info.camMatrix, self.ros_overhead_cam_info.distCoeffs)
if (retval > 0):
if(m.name=="leeward_mid_panel_marker_5"):
print "Found tag: " + m.name
try:
#o_pose = self.listener.lookupTransform(m.name, object_name, rospy.Time(0))
o_pose = rox_msg.msg2transform(m.pose).inv()
print object_name
print o_pose
print ""
print "o_pose"
print o_pose.p
print o_pose.R
Ra, b = cv2.Rodrigues(rvec)
a_pose=rox.Transform(Ra,tvec)
print "a_pose"
print a_pose.p
print a_pose.R
tag_object_poses[m.name]=c_pose*(a_pose*o_pose)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
#TODO: merge tag data if multiple tags detected
if len(tag_object_poses) > 0:
p=PoseWithCovarianceStamped()
p.pose.pose=rox_msg.transform2pose_msg(tag_object_poses.itervalues().next())
p.header.stamp=now
p.header.frame_id=self.frame_id
r=RecognizedObject()
r.header.stamp=now
r.header.frame_id=self.frker, aruco_dict)
board = cv2.aruco.GridBoard_createame_id
r.type.key=object_name
r.confidence=1
r.pose=p
r_array.objects.append(r)
"""for o in self.object_names:
try:
(trans,rot) = self.listener.lookupTransform("/world", o, rospy.Time(0))
print o
print trans
print rot
print ""
p=PoseWithCovarianceStamped()
p.pose.pose=Pose(Point(trans[0], trans[1], trans[2]), Quaternion(rot[0], rot[1], rot[2], rot[3]))
p.header.stamp=now
p.header.frame_id=self.frame_id
r=RecognizedObject()
r.header.stamp=now
r.header.frame_id=self.frame_id
r.type.key=o
r.confidence=1
r.pose=p
r_array.objects.append(r)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue"""
result = ObjectRecognitionResult()
result.recognized_objects=r_array
'''
self.gripper_server.set_succeeded(result=result)
class PBVSPlacementController(object):
def __init__(self, controller_commander, urdf_xml_string, srdf_xml_string,
camera_image_topic, camera_trigger_topic, camera_info_topic):
self.controller_commander = controller_commander
self.planner = Planner(controller_commander, urdf_xml_string,
srdf_xml_string)
self.tf_listener = PayloadTransformListener()
self.camera_info = self.get_camera_info(camera_info_topic)
self.img_queue = Queue.Queue(1)
self.camera_trigger = rospy.ServiceProxy(camera_trigger_topic, Trigger)
self.camera_sub = rospy.Subscriber(camera_image_topic, Image,
self._ros_img_cb)
self.controller_state_sub = rospy.Subscriber("controller_state",
controllerstate,
self.ft_cb)
self.FTdata = None
self.ft_flag = False
self.FTdata_0 = self.FTdata
# Compliance controller parameters
self.F_d_set1 = -100
self.F_d_set2 = -200
self.Kc = 0.00002
self.client = actionlib.SimpleActionClient(
"joint_trajectory_action", FollowJointTrajectoryAction)
self.K_pbvs = 0.25
def _ros_img_cb(self, ros_img):
img1 = CvBridge().imgmsg_to_cv2(ros_img)
try:
self.img_queue.get_nowait()
except Queue.Empty:
pass
self.img_queue.put_nowait(img1)
def set_goal(self, params_msg):
self.desired_transform = rox_msg.msg2transform(
params_msg.desired_transform)
self.desired_camera2_transform = rox_msg.msg2transform(
params_msg.desired_camera2_transform)
self.markers = self.get_all_payload_markers()
self.fixed_marker = self.markers[
self.desired_transform.parent_frame_id]
self.payload_marker = self.markers[
self.desired_transform.child_frame_id]
self.aruco_dict = self.get_aruco_dictionary(self.fixed_marker)
self.stage1_tol_p = params_msg.stage1_tol_p
self.stage1_tol_r = params_msg.stage1_tol_r
self.stage2_tol_p = params_msg.stage2_tol_p
self.stage2_tol_r = params_msg.stage2_tol_r
self.stage3_tol_p = params_msg.stage3_tol_p
self.stage3_tol_r = params_msg.stage3_tol_r
self.stage1_kp = params_msg.stage1_kp
self.stage2_kp = params_msg.stage2_kp
self.stage3_kp = params_msg.stage3_kp
self.stage2_z_offset = params_msg.stage2_z_offset
self.force_ki = params_msg.force_ki
def unpack_wrench(w):
return np.array([
w.torque.x, w.torque.y, w.torque.z, w.force.x, w.force.y,
w.force.z
])
self.abort_force = unpack_wrench(params_msg.abort_force)
self.placement_force = unpack_wrench(params_msg.placement_force)
self._aborted = False
def compute_step_gripper_target_pose(self,
img,
Kp,
no_z=False,
z_offset=0):
fixed_marker_transform, payload_marker_transform, error_transform = self.compute_error_transform(
img)
if no_z:
error_transform.p[2] = 0
else:
error_transform.p[2] -= z_offset
gripper_to_camera_tf = self.tf_listener.lookupTransform(
"vacuum_gripper_tool", "gripper_camera_2", rospy.Time(0))
world_to_vacuum_gripper_tool_tf = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
#Scale by Kp
k, theta = rox.R2rot(error_transform.R)
r = np.multiply(k * theta, Kp[0:3])
r_norm = np.linalg.norm(r)
if (r_norm < 1e-6):
error_transform2_R = np.eye(3)
else:
error_transform2_R = rox.rot(r / r_norm, r_norm)
error_transform2_p = np.multiply(error_transform.p, (Kp[3:6]))
error_transform2 = rox.Transform(error_transform2_R,
error_transform2_p)
gripper_to_fixed_marker_tf = gripper_to_camera_tf * fixed_marker_transform
gripper_to_desired_fixed_marker_tf = gripper_to_fixed_marker_tf * error_transform2
#print gripper_to_fixed_marker_tf
ret = world_to_vacuum_gripper_tool_tf * (
gripper_to_desired_fixed_marker_tf *
gripper_to_fixed_marker_tf.inv()).inv()
#print world_to_vacuum_gripper_tool_tf
#print ret
#print error_transform
return ret, error_transform
def compute_error_transform(self, img):
fixed_marker_transform = self.detect_marker(img, self.fixed_marker)
payload_marker_transform = self.detect_marker(img, self.payload_marker)
tag_to_tag_transform = fixed_marker_transform.inv(
) * payload_marker_transform
error_transform = tag_to_tag_transform * self.desired_transform.inv()
return fixed_marker_transform, payload_marker_transform, error_transform
def receive_image(self):
try:
self.img_queue.get_nowait()
except Queue.Empty:
pass
self.camera_trigger()
return self.img_queue.get(timeout=10.0)
def get_all_payload_markers(self):
payload_array = rospy.wait_for_message("payload",
PayloadArray,
timeout=1)
markers = dict()
for p in payload_array.payloads:
for m in p.markers:
markers[m.name] = m.marker
for p in payload_array.link_markers:
for m in p.markers:
markers[m.name] = m.marker
return markers
def get_aruco_dictionary(self, marker):
if not hasattr(cv2.aruco, marker.dictionary):
raise ValueError("Invalid aruco-dict value")
aruco_dict_id = getattr(cv2.aruco, marker.dictionary)
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id)
return aruco_dict
def get_aruco_gridboard(self, marker):
#Create grid board representing the calibration target
aruco_dict = self.get_aruco_dictionary(marker)
if isinstance(marker.dictionary, basestring):
if not marker.dictionary.startswith('DICT_'):
raise ValueError("Invalid aruco-dict value")
elif isinstance(marker.dictionary, int):
aruco_dict = cv2.aruco.Dictionary_get(marker.dictionary)
else:
aruco_dict_id = marker.dictionary
board=cv2.aruco.GridBoard_create(marker.markersX, marker.markersY, \
marker.markerLength, marker.markerSpacing, aruco_dict,\
marker.firstMarker)
return board
def get_camera_info(self, camera_info_topic):
return rospy.wait_for_message(camera_info_topic, CameraInfo, timeout=1)
def detect_marker(self, img, marker):
camMatrix = np.reshape(self.camera_info.K, (3, 3))
distCoeffs = np.array(self.camera_info.D)
parameters = cv2.aruco.DetectorParameters_create()
parameters.cornerRefinementWinSize = 32
parameters.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_CONTOUR
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
img, self.aruco_dict, parameters=parameters)
img2 = cv2.aruco.drawDetectedMarkers(img, corners, ids)
img3 = cv2.resize(img2, (0, 0), fx=0.25, fy=0.25)
cv2.imshow("", img3)
cv2.waitKey(1)
board = self.get_aruco_gridboard(marker)
retval, rvec, tvec = cv2.aruco.estimatePoseBoard(
corners, ids, board, camMatrix, distCoeffs)
if (retval <= 0):
#cv2.waitKey()
raise Exception("Invalid image")
Ra, b = cv2.Rodrigues(rvec)
a_pose = rox.Transform(Ra, tvec)
frame_with_markers_and_axis = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
frame_with_markers_and_axis = cv2.aruco.drawAxis(
frame_with_markers_and_axis, camMatrix, distCoeffs, rvec, tvec,
0.2)
frame_with_markers_and_axis = cv2.resize(frame_with_markers_and_axis,
(0, 0),
fx=0.25,
fy=0.25)
cv2.imshow("transform", frame_with_markers_and_axis)
cv2.waitKey(1)
return a_pose
def pbvs(self,
kp,
tols_p,
tols_r,
abort_force,
max_iters=25,
no_z=False,
z_offset=0):
i = 0
while True:
if i > max_iters:
raise Exception("Placement controller timeout")
if self._aborted:
raise Exception("Operation aborted")
img = self.receive_image()
target_pose, err = self.compute_step_gripper_target_pose(
img, kp, no_z=no_z, z_offset=z_offset)
err_p = np.linalg.norm(err.p)
if no_z:
err_p = np.linalg.norm(err.p[0:2])
err_r = np.abs(rox.R2rot(err.R)[1])
if err_p < tols_p and err_r < tols_r:
return err_p, err_r
#target_pose.p[1]-=0.01 # Offset a little to avoid panel overlap
plan = self.planner.trajopt_plan(
target_pose, json_config_name="panel_placement")
if self._aborted:
raise Exception("Operation aborted")
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [],
abort_force)
self.controller_commander.execute_trajectory(plan)
i += 1
def pbvs_stage1(self):
print "stage 1"
return self.pbvs(self.stage1_kp,
self.stage1_tol_p,
self.stage1_tol_r,
self.abort_force,
no_z=True)
def pbvs_stage2(self):
print "stage 2"
return self.pbvs(self.stage2_kp,
self.stage2_tol_p,
self.stage2_tol_r,
self.abort_force,
z_offset=self.stage2_z_offset)
def pbvs_stage3(self):
print "stage 3"
return self.pbvs(self.stage3_kp, self.stage3_tol_p, self.stage3_tol_r,
self.abort_force)
def abort(self):
self._aborted = True
self.controller_commander.stop_trajectory()
# Added by YC
def ft_cb(self, data):
self.FTdata = np.array([data.ft_wrench.torque.x,data.ft_wrench.torque.y,data.ft_wrench.torque.z,\
data.ft_wrench.force.x,data.ft_wrench.force.y,data.ft_wrench.force.z])
self.ft_flag = True
def trapezoid_gen(self, target, current_joint_angles, acc, dcc, vmax):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
goal.trajectory.header.frame_id = '/world'
dist = np.array(target - current_joint_angles)
xf = max(abs(dist))
#print "Dist:====================", xf
[x0, v0, a0, ta, tb, tf] = trapgen(0, xf, 0, 0, vmax, acc, dcc, 0)
[xa, va, aa, ta, tb, tf] = trapgen(0, xf, 0, 0, vmax, acc, dcc, ta)
[xb, vb, ab, ta, tb, tf] = trapgen(0, xf, 0, 0, vmax, acc, dcc, tb)
p1 = JointTrajectoryPoint()
p1.positions = current_joint_angles
p1.time_from_start = rospy.Duration(0)
p2 = JointTrajectoryPoint()
p2.positions = np.array(p1.positions) + dist * xa
p2.time_from_start = rospy.Duration(ta)
p3 = JointTrajectoryPoint()
p3.positions = np.array(p1.positions) + dist * xb
p3.time_from_start = rospy.Duration(tb)
p4 = JointTrajectoryPoint()
p4.positions = target
p4.velocities = np.zeros((6, ))
p4.accelerations = np.zeros((6, ))
p4.time_from_start = rospy.Duration(tf)
goal.trajectory.points.append(p1)
goal.trajectory.points.append(p2)
goal.trajectory.points.append(p3)
goal.trajectory.points.append(p4)
return goal
def test(self):
img = cv2.imread('12_57_46_523.png')
img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
fixed_marker_transform, payload_marker_transform, error_transform = self.compute_error_transform(
img)
tag_to_tag_transform = fixed_marker_transform.inv(
) * payload_marker_transform
print "Test1:", fixed_marker_transform, payload_marker_transform
print "Test2:", tag_to_tag_transform
print "Test3:", error_transform
def pbvs_jacobian(self):
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [], [])
tvec_err = [100, 100, 100]
rvec_err = [100, 100, 100]
self.FTdata_0 = self.FTdata
error_transform = rox.Transform(rox.rpy2R([2, 2, 2]),
np.array([100, 100, 100]))
FT_data_ori = []
FT_data_biased = []
err_data_p = []
err_data_rpy = []
joint_data = []
time_data = []
#TODO: should listen to stage_3_tol_r not 1 degree
print self.desired_camera2_transform
#R_desired_cam = self.desired_camera2_transform.R.dot(self.desired_transform.R)
#R_desired_cam = R_desired_cam.dot(self.desired_camera2_transform.R.transpose())
#t_desired_cam = -self.desired_camera2_transform.R.dot(self.desired_transform.p)
while (error_transform.p[2] > 0.01 or np.linalg.norm(
[error_transform.p[0], error_transform.p[1]]) > self.stage3_tol_p
or
np.linalg.norm(rox.R2rpy(error_transform.R)) > np.deg2rad(1)):
img = self.receive_image()
fixed_marker_transform, payload_marker_transform, error_transform = self.compute_error_transform(
img)
#print self.desired_transform.R.T, -fixed_marker_transform.R.dot(self.desired_transform.p)
R_desired_cam = fixed_marker_transform.R.dot(
self.desired_transform.R)
R_desired_cam = R_desired_cam.dot(
fixed_marker_transform.R.transpose())
t_desired_cam = -fixed_marker_transform.R.dot(
self.desired_transform.p)
# Compute error directly in the camera frame
observed_R_difference = np.dot(
payload_marker_transform.R,
fixed_marker_transform.R.transpose())
k, theta = rox.R2rot(
np.dot(observed_R_difference, R_desired_cam.transpose())
) #np.array(rox.R2rpy(rvec_err1))
rvec_err1 = k * theta
observed_tvec_difference = fixed_marker_transform.p - payload_marker_transform.p
tvec_err1 = -fixed_marker_transform.R.dot(
self.desired_transform.p) - observed_tvec_difference
#print 'SPOT: ',tvec_err1, rvec_err1
# Map error to the robot spatial velocity
world_to_camera_tf = self.tf_listener.lookupTransform(
"world", "gripper_camera_2", rospy.Time(0))
camera_to_link6_tf = self.tf_listener.lookupTransform(
"gripper_camera_2", "link_6", rospy.Time(0))
t21 = -np.dot(
rox.hat(
np.dot(world_to_camera_tf.R,
(camera_to_link6_tf.p -
payload_marker_transform.p.reshape((1, 3))).T)),
world_to_camera_tf.R) #np.zeros((3,3))#
# v = R_oc(vc)c + R_oc(omeega_c)_c x (r_pe)_o = R_oc(vc)c - (r_pe)_o x R_oc(omeega_c)_c
tvec_err = t21.dot(rvec_err1).reshape(
(3, )) + world_to_camera_tf.R.dot(tvec_err1).reshape((3, ))
# omeega = R_oc(omeega_c)_c
rvec_err = world_to_camera_tf.R.dot(rvec_err1).reshape((3, ))
tvec_err = np.clip(tvec_err, -0.2, 0.2)
rvec_err = np.clip(rvec_err, -np.deg2rad(5), np.deg2rad(5))
if tvec_err[2] < 0.03:
rospy.loginfo("Only Compliance Control")
tvec_err[2] = 0
rot_err = rox.R2rpy(error_transform.R)
rospy.loginfo("tvec difference: %f, %f, %f", error_transform.p[0],
error_transform.p[1], error_transform.p[2])
rospy.loginfo("rvec difference: %f, %f, %f", rot_err[0],
rot_err[1], rot_err[2])
dx = -np.concatenate((rvec_err, tvec_err)) * self.K_pbvs
print np.linalg.norm([error_transform.p[0], error_transform.p[1]])
print np.linalg.norm(rox.R2rpy(error_transform.R))
# Compliance Force Control
if (not self.ft_flag):
raise Exception("havent reached FT callback")
# Compute the exteranl force
FTread = self.FTdata - self.FTdata_0 # (F)-(F0)
print '================ FT1 =============:', FTread
print '================ FT2 =============:', self.FTdata
if FTread[-1] > (self.F_d_set1 + 50):
F_d = self.F_d_set1
else:
F_d = self.F_d_set2
if (self.FTdata == 0).all():
rospy.loginfo("FT data overflow")
dx[-1] += self.K_pbvs * 0.004
else:
tx_correct = 0
if abs(self.FTdata[0]) > 80:
tx_correct = 0.0002 * (abs(self.FTdata[0]) - 80)
Vz = self.Kc * (F_d - FTread[-1]) + tx_correct
dx[-1] = dx[-1] + Vz
print "dx:", dx
current_joint_angles = self.controller_commander.get_current_joint_values(
)
joints_vel = QP_abbirb6640(
np.array(current_joint_angles).reshape(6, 1), np.array(dx))
goal = self.trapezoid_gen(
np.array(current_joint_angles) + joints_vel.dot(1),
np.array(current_joint_angles), 0.008, 0.008,
0.015) #acc,dcc,vmax)
print "joints_vel:", joints_vel
self.client.wait_for_server()
self.client.send_goal(goal)
self.client.wait_for_result()
res = self.client.get_result()
if (res.error_code != 0):
raise Exception("Trajectory execution returned error")
FT_data_ori.append(self.FTdata)
FT_data_biased.append(FTread)
err_data_p.append(error_transform.p)
err_data_rpy.append(rot_err)
joint_data.append(current_joint_angles)
time_data.append(time.time())
filename_pose = "/home/rpi-cats/Desktop/YC/Data/Panel2_Placement_In_Nest_Pose_" + str(
time.time()) + ".mat"
scipy.io.savemat(filename_pose,
mdict={
'FT_data_ori': FT_data_ori,
'FT_data_biased': FT_data_biased,
'err_data_p': err_data_p,
'err_data_rpy': err_data_rpy,
'joint_data': joint_data,
'time_data': time_data
})
rospy.loginfo("End ====================")
def __init__(self,
frame_id="world",
action_ns_1="recognize_objects",
action_ns_2="recognize_objects_gripper"):
self.bridge = CvBridge()
self.overhead_server = SimpleActionServer(
action_ns_1,
ObjectRecognitionAction,
execute_cb=self.execute_overhead_callback)
self.gripper_server = SimpleActionServer(
action_ns_2,
ObjectRecognitionAction,
execute_cb=self.execute_gripper_callback)
self.recognized_objects = dict()
self.listener = PayloadTransformListener()
self.frame_id = "world"
self.ros_image = None
self.ros_image_stamp = rospy.Time(0)
self.last_ros_image_stamp = rospy.Time(0)
self.ros_overhead_cam_info = CameraInfoStorage()
self.ros_gripper_1_cam_info = CameraInfoStorage()
self.ros_gripper_2_cam_info = CameraInfoStorage()
#TODO: Handle compressed images vs uncompressed images better. Note that
#the uncompressed 20 MP color images don't seem to be received properly.
if not "compressed-image" in sys.argv:
self.ros_overhead_img_sub = rospy.Subscriber(
'/overhead_camera/image', Image,
self.ros_raw_overhead_image_cb)
self.ros_gripper_1_img_sub = rospy.Subscriber(
'/gripper_camera_1/image', Image,
self.ros_raw_gripper_1_image_cb)
self.ros_gripper_2_img_sub = rospy.Subscriber(
'/gripper_camera_2/image', Image,
self.ros_raw_gripper_2_image_cb)
else:
self.ros_overhead_img_sub = rospy.Subscriber(
'/overhead_camera/image/compressed', CompressedImage,
self.ros_image_cb)
self.ros_gripper_1_img_sub = rospy.Subscriber(
'/gripper_camera_1/image/compressed', CompressedImage,
self.ros_image_cb)
self.ros_gripper_2_img_sub = rospy.Subscriber(
'/gripper_camera_2/image/compressed', CompressedImage,
self.ros_image_cb)
self.ros_overhead_trigger = rospy.ServiceProxy(
'/overhead_camera/trigger', Trigger)
self.ros_overhead_cam_info_sub = rospy.Subscriber(
'/overhead_camera/camera_info', CameraInfo, self.ros_cam_info_cb)
self.ros_gripper_1_trigger = rospy.ServiceProxy(
'/gripper_camera_1/trigger', Trigger)
self.ros_gripper_2_trigger = rospy.ServiceProxy(
'/gripper_camera_2/trigger', Trigger)
self.ros_gripper_1_cam_info_sub = rospy.Subscriber(
'/gripper_camera_1/camera_info', CameraInfo, self.ros_cam_info_cb)
self.ros_gripper_2_cam_info_sub = rospy.Subscriber(
'/gripper_camera_2/camera_info', CameraInfo, self.ros_cam_info_cb)
#self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
self.payloads = dict()
self.link_markers = dict()
self.payloads_lock = threading.Lock()
self.payloads_sub = rospy.Subscriber("payload", PayloadArray,
self._payload_msg_cb)
class ProcessController(object):
def __init__(self, disable_ft=False):
self.lock = threading.Lock()
urdf_xml_string = rospy.get_param("robot_description")
srdf_xml_string = rospy.get_param("robot_description_semantic")
self.urdf = URDF.from_parameter_server()
self.overhead_vision_client = actionlib.SimpleActionClient(
"recognize_objects", ObjectRecognitionAction)
self.rapid_node = rapid_node_pkg.RAPIDCommander()
self.controller_commander = ControllerCommander()
self.state = 'init'
self.current_target = None
self.current_payload = None
self.available_payloads = {
'leeward_mid_panel': 'leeward_mid_panel',
'leeward_tip_panel': 'leeward_tip_panel'
}
self.desired_controller_mode = self.controller_commander.MODE_AUTO_TRAJECTORY
self.speed_scalar = 1.0
self.disable_ft = disable_ft
self.tf_listener = PayloadTransformListener()
self._process_state_pub = rospy.Publisher("process_state",
ProcessState,
queue_size=100,
latch=True)
self.publish_process_state()
self.placement_client = actionlib.SimpleActionClient(
'placement_step', PBVSPlacementAction)
#self.placement_client.wait_for_server()
self.update_payload_pose_srv = rospy.ServiceProxy(
"update_payload_pose", UpdatePayloadPose)
self.get_payload_array_srv = rospy.ServiceProxy(
"get_payload_array", GetPayloadArray)
self._goal_handle = None
self._goal_handle_lock = threading.RLock()
self.subprocess_handle = None
self.plan_dictionary = {}
self.process_starts = {}
self.process_index = None
self.process_states = [
"reset_position", "pickup_prepare", "pickup_lower",
"pickup_grab_first_step", "pickup_grab_second_step",
"pickup_raise", "transport_payload", "place_payload",
"gripper_release"
]
self.planner = Planner(self.controller_commander, urdf_xml_string,
srdf_xml_string)
def _vision_get_object_pose(self, key):
self.overhead_vision_client.wait_for_server()
goal = ObjectRecognitionGoal(False, [-1e10, 1e10, -1e10, 1e10])
self.overhead_vision_client.send_goal(goal)
self.overhead_vision_client.wait_for_result()
ret = self.overhead_vision_client.get_result()
for r in ret.recognized_objects.objects:
if r.type.key == key:
rox_pose = rox_msg.msg2transform(r.pose.pose.pose)
rox_pose.parent_frame_id = r.pose.header.frame_id
rox_pose.child_frame_id = key
return rox_pose
raise Exception("Requested object not found")
def _vision_get_object_gripper_target_pose(self, key):
object_pose = self._vision_get_object_pose(key)
tag_rel_pose = self.tf_listener.lookupTransform(
key, key + "_gripper_target", rospy.Time(0))
return object_pose * tag_rel_pose, object_pose
def _tf_get_object_gripper_target_pose(self, key):
payload = self._get_payload(key)
if payload.confidence < 0.8:
raise Exception("Payload confidence too low for tf lookup")
object_pose = self.tf_listener.lookupTransform("/world", key,
rospy.Time(0))
tag_rel_pose = self.tf_listener.lookupTransform(
key, key + "_gripper_target", rospy.Time(0))
return object_pose * tag_rel_pose, object_pose
def get_payload_pickup_ft_threshold(self, payload):
if self.disable_ft:
return []
return self._get_payload(
payload).gripper_targets[0].pickup_ft_threshold
def get_state(self):
return self.state
def get_current_pose(self):
return self.controller_commander.get_current_pose_msg()
def cancel_step(self, goal=None):
with self._goal_handle_lock:
self.stop_motion()
def stop_motion(self):
with self._goal_handle_lock:
self.controller_commander.stop_trajectory()
def plan_rewind_motion(self, goal):
no_rewind_list = [None, 0, 4, 5, 8]
self._begin_step(goal)
if (self.process_index not in no_rewind_list):
try:
if (self.process_index == 3):
self.process_index -= 1
rewind_target_pose = self.process_starts[
self.process_states[self.process_index]]
else:
rewind_target_pose = self.process_starts[
self.process_states[self.process_index]]
path = self._plan(rewind_target_pose,
config="reposition_robot",
smoother_config="reposition_robot_smoother")
self.plan_dictionary['rewind_motion'] = path
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
else:
self._step_failed(
"Rewind Unavailable, Please Manually Reposition Robot", goal)
def move_rewind_motion(self, mode, goal):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(
mode, self.speed_scalar, [], [])
path = self.plan_dictionary['rewind_motion']
self._execute_path(path, goal)
self.process_index -= 1
self.state = self.process_states[self.process_index]
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def plan_reset_position(self, goal=None):
self._begin_step(goal)
try:
Q = [0.02196692, -0.10959773, 0.99369529, -0.00868731]
P = [1.8475985, -0.04983688, 0.82486047]
rospy.loginfo("Planning to reset position")
self.state = "reset_position"
self.process_index = 0
pose_target = rox.Transform(rox.q2R(Q), np.copy(P))
path = self._plan(pose_target,
config="reposition_robot",
smoother_config="reposition_robot_smoother")
self.plan_dictionary['reset_position'] = path
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def move_reset_position(self,
mode=ControllerCommander.MODE_AUTO_TRAJECTORY,
goal=None):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, self.speed_scalar, [], [])
self.controller_commander.set_controller_mode(
mode, self.speed_scalar, [], [])
path = self.plan_dictionary['reset_position']
self._execute_path(path, goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def place_panel(self, target_payload, goal=None):
self._begin_step(goal)
def done_cb(status, result):
rospy.loginfo("ibvs placement generated success")
if (goal is not None):
if status == actionlib.GoalStatus.SUCCEEDED:
self._step_complete(goal)
else:
with self._goal_handle_lock:
if self._goal_handle == goal:
self._goal_handle = None
res = ProcessStepResult()
res.state = self.state
res.target = self.current_target if self.current_target is not None else ""
res.payload = self.current_payload if self.current_payload is not None else ""
res.error_msg = str(result.error_msg)
goal.set_aborted(result=res)
rospy.loginfo("pbvs placement generated: %s",
result.error_msg)
self.process_index = 7
self.process_starts[self.process_states[
self.process_index]] = self.get_current_pose()
with self._goal_handle_lock:
placement_goal = PBVSPlacementGoal()
placement_goal.desired_transform = self.load_placement_target_config(
target_payload)
placement_goal.stage1_kp = np.array([0.9] * 6)
placement_goal.stage2_kp = np.array([0.9] * 6)
placement_goal.stage3_kp = np.array([0.5] * 6)
placement_goal.stage1_tol_p = 0.05
placement_goal.stage1_tol_r = np.deg2rad(1)
placement_goal.stage2_tol_p = 0.05
placement_goal.stage2_tol_r = np.deg2rad(1)
placement_goal.stage3_tol_p = 0.001
placement_goal.stage3_tol_r = np.deg2rad(0.2)
placement_goal.stage2_z_offset = 0.05
placement_goal.abort_force = Wrench(Vector3(500, 500, 500),
Vector3(100, 100, 100))
placement_goal.placement_force = Wrench(Vector3(0, 0, 300),
Vector3(0, 0, 0))
placement_goal.force_ki = np.array([1e-6] * 6)
client_handle = self.placement_client.send_goal(placement_goal,
done_cb=done_cb)
def plan_pickup_prepare(self, target_payload, goal=None):
self._begin_step(goal)
try:
rospy.loginfo("Begin pickup_prepare for payload %s",
target_payload)
object_target, object_pose = self._vision_get_object_gripper_target_pose(
target_payload)
self._update_payload_pose(target_payload,
object_pose,
parent_frame_id="pickup_nest",
confidence=0.8)
rospy.loginfo("Found payload %s at pose %s", target_payload,
object_target)
self.pose_target = copy.deepcopy(object_target)
pose_target = self.pose_target
pose_target.p[2] += 0.5
rospy.loginfo("Prepare pickup %s at pose %s", target_payload,
object_target)
print pose_target.p
path = self._plan(pose_target,
config="reposition_robot",
smoother_config="reposition_robot_smoother")
self.current_target = target_payload
self.state = "plan_pickup_prepare"
self.plan_dictionary['pickup_prepare'] = path
#rospy.loginfo("Finish pickup prepare for payload %s", target_payload)
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def move_pickup_prepare(self,
mode=ControllerCommander.MODE_AUTO_TRAJECTORY,
goal=None):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(
mode, self.speed_scalar, [], [])
result = None
self.state = "pickup_prepare"
self.process_index = 1
self.process_starts[self.process_states[
self.process_index]] = self.get_current_pose()
plan = self.plan_dictionary['pickup_prepare']
self._execute_path(plan, goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def plan_pickup_lower(self, goal=None):
#TODO: check change state and target
self._begin_step(goal)
try:
rospy.loginfo("Begin pickup_lower for payload %s",
self.current_target)
object_target, _ = self._tf_get_object_gripper_target_pose(
self.current_target)
pose_target2 = copy.deepcopy(object_target)
pose_target2.p[2] += 0.3
print pose_target2.p
#path=self.controller_commander.compute_cartesian_path(pose_target2, avoid_collisions=False)
path = self._plan(pose_target2, config="reposition_robot_short")
self.state = "plan_pickup_lower"
self.plan_dictionary['pickup_lower'] = path
rospy.loginfo("Finish pickup_lower for payload %s",
self.current_target)
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def move_pickup_lower(self,
mode=ControllerCommander.MODE_AUTO_TRAJECTORY,
goal=None):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(
mode, 0.8 * self.speed_scalar, [],
self.get_payload_pickup_ft_threshold(self.current_target))
result = None
rospy.loginfo("moving_pickup_lower")
if (self.state != "plan_pickup_lower"):
self.plan_pickup_lower()
self.state = "pickup_lower"
self.process_index = 2
self.process_starts[self.process_states[
self.process_index]] = self.get_current_pose()
path = self.plan_dictionary['pickup_lower']
self._execute_path(path, goal)
#self.execute_trajectory_action.wait_for_result()
#self.controller_commander.execute(self.plan_dictionary['pickup_lower'])
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def plan_pickup_grab_first_step(self, goal=None):
#TODO: check change state and target
self._begin_step(goal)
try:
rospy.loginfo("Begin pickup_grab for payload %s",
self.current_target)
self.object_target, _ = self._tf_get_object_gripper_target_pose(
self.current_target)
pose_target2 = copy.deepcopy(self.object_target)
pose_target2.p[2] -= 0.15
#path=self.controller_commander.compute_cartesian_path(pose_target2, avoid_collisions=False)
path = self._plan(pose_target2, config="panel_pickup")
self.state = "plan_pickup_grab_first_step"
self.plan_dictionary['pickup_grab_first_step'] = path
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def move_pickup_grab_first_step(self,
mode=ControllerCommander.
MODE_AUTO_TRAJECTORY,
goal=None):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(mode, 0.8*self.speed_scalar, [],\
self.get_payload_pickup_ft_threshold(self.current_target))
result = None
if (self.state != "plan_pickup_grab_first_step"):
self.plan_pickup_grab_first_step()
self.state = "pickup_grab_first_step"
self.process_index = 3
self.process_starts[self.process_states[
self.process_index]] = self.get_current_pose()
path = self.plan_dictionary['pickup_grab_first_step']
self._execute_path(path, goal, ft_stop=True)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def plan_pickup_grab_second_step(self, goal=None):
self._begin_step(goal)
try:
self.rapid_node.set_digital_io("Vacuum_enable", 1)
time.sleep(1)
#TODO: check vacuum feedback to make sure we have the panel
world_to_panel_tf = self.tf_listener.lookupTransform(
"world", self.current_target, rospy.Time(0))
world_to_gripper_tf = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
panel_to_gripper_tf = world_to_gripper_tf.inv() * world_to_panel_tf
#Add extra cushion for spring extension. This should be a parameter somewhere rather than hard coded.
if (self.current_target == "leeward_mid_panel"):
panel_to_gripper_tf.p[2] += 0.07
else:
panel_to_gripper_tf.p[2] += 0.05
self.current_payload = self.current_target
self.current_target = None
self._update_payload_pose(self.current_payload,
panel_to_gripper_tf,
"vacuum_gripper_tool", 0.5)
self.controller_commander.set_controller_mode(
self.controller_commander.MODE_HALT, 1, [], [])
time.sleep(1)
pose_target2 = copy.deepcopy(self.object_target)
pose_target2.p[2] += 0.20
#path=self.controller_commander.compute_cartesian_path(pose_target2, avoid_collisions=False)
path = self._plan(pose_target2, config="panel_pickup")
self.state = "plan_pickup_grab_second_step"
self.plan_dictionary['pickup_grab_second_step'] = path
rospy.loginfo("Finish pickup_grab for payload %s",
self.current_payload)
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def move_pickup_grab_second_step(self,
mode=ControllerCommander.
MODE_AUTO_TRAJECTORY,
goal=None):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(
mode, 0.8 * self.speed_scalar, [], [])
result = None
if (self.state != "plan_pickup_grab_second_step"):
self.plan_pickup_grab_second_step()
self.state = "pickup_grab_second_step"
self.process_index = 4
self.process_starts[self.process_states[
self.process_index]] = self.get_current_pose()
path = self.plan_dictionary['pickup_grab_second_step']
self._execute_path(path, goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def plan_pickup_raise(self, goal=None):
#TODO: check change state and target
self._begin_step(goal)
try:
rospy.loginfo("Begin pickup_raise for payload %s",
self.current_payload)
#Just use gripper position for now, think up a better way in future
object_target = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
pose_target2 = copy.deepcopy(object_target)
pose_target2.p[2] += 0.35
#pose_target2.p = np.array([-0.02285,-1.840,1.0])
#pose_target2.R = rox.q2R([0.0, 0.707, 0.707, 0.0])
path = self._plan(pose_target2, config="transport_panel_short")
self.state = "plan_pickup_raise"
self.plan_dictionary['pickup_raise'] = path
rospy.loginfo("Finish pickup_raise for payload %s",
self.current_target)
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def move_pickup_raise(self,
mode=ControllerCommander.MODE_AUTO_TRAJECTORY,
goal=None):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(
mode, 0.8 * self.speed_scalar, [], [])
result = None
if (self.state != "plan_pickup_raise"):
self.plan_pickup_raise()
self.state = "pickup_raise"
self.process_index = 5
self.process_starts[self.process_states[
self.process_index]] = self.get_current_pose()
path = self.plan_dictionary['pickup_raise']
self._execute_path(path, goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def plan_transport_payload(self, target, goal=None):
#TODO: check state and payload
rospy.loginfo("Begin transport_panel for payload %s to %s",
self.current_payload, target)
self._begin_step(goal)
try:
panel_target_pose = self.tf_listener.lookupTransform(
"world", target, rospy.Time(0))
panel_gripper_pose = self.tf_listener.lookupTransform(
self.current_payload, "vacuum_gripper_tool", rospy.Time(0))
pose_target = panel_target_pose * panel_gripper_pose
#pose_target.p = [1.97026484647054, 1.1179574262842452, 0.12376598588449844]
#pose_target.R = np.array([[-0.99804142, 0.00642963, 0.06222524], [ 0.00583933, 0.99993626, -0.00966372], [-0.06228341, -0.00928144, -0.99801535]])
#pose_target.p[1] += -0.35
pose_target.p[2] += 0.40
#plan=self.controller_commander.plan(pose_target)
plan = self._plan(pose_target,
config="transport_panel",
smoother_config="transport_panel_smoother")
self.current_target = target
self.state = "plan_transport_payload"
self.plan_dictionary['transport_payload'] = plan
rospy.loginfo("Finish transport_panel for payload %s to %s",
self.current_payload, target)
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def move_transport_payload(self,
mode=ControllerCommander.MODE_AUTO_TRAJECTORY,
goal=None):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(
mode, 0.8 * self.speed_scalar, [], [])
self.state = "transport_payload"
self.process_index = 6
self.process_starts[self.process_states[
self.process_index]] = self.get_current_pose()
plan = self.plan_dictionary['transport_payload']
self._execute_path(plan, goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def load_placement_target_config(self, target_payload):
if (target_payload == "leeward_mid_panel"):
transform_fname = os.path.join(
rospkg.RosPack().get_path(
'rpi_arm_composites_manufacturing_process'), 'config',
'leeward_mid_panel_marker_transform.yaml')
elif (target_payload == "leeward_tip_panel"):
transform_fname = os.path.join(
rospkg.RosPack().get_path(
'rpi_arm_composites_manufacturing_process'), 'config',
'leeward_tip_panel_marker_transform.yaml')
desired_transform_msg = TransformStamped()
with open(transform_fname, 'r') as f:
transform_yaml = yaml.load(f)
genpy.message.fill_message_args(desired_transform_msg, transform_yaml)
return desired_transform_msg
def plan_gripper_release(self, goal=None):
self._begin_step(goal)
try:
self.rapid_node.set_digital_io("Vacuum_enable", 0)
#time.sleep(1)
self.controller_commander.set_controller_mode(
ControllerCommander.MODE_HALT, 0.8 * self.speed_scalar, [], [])
#TODO: check vacuum feedback to make sure we have the panel
pose_target2 = self.controller_commander.compute_fk()
pose_target2.p[2] += 0.25
#self.current_payload=self.current_target
gripper_to_panel_tf = self.tf_listener.lookupTransform(
"vacuum_gripper_tool", self.current_payload, rospy.Time(0))
world_to_gripper_tf = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
world_to_panel_nest_tf = self.tf_listener.lookupTransform(
"world", "panel_nest", rospy.Time(0))
panel_to_nest_tf = world_to_panel_nest_tf.inv(
) * world_to_gripper_tf * gripper_to_panel_tf
self._update_payload_pose(self.current_payload, panel_to_nest_tf,
"panel_nest", 0.5)
self.current_payload = None
self.current_target = None
time.sleep(1)
path = self._plan(pose_target2, config="panel_pickup")
self.state = "plan_gripper_release"
self.plan_dictionary['gripper_release'] = path
rospy.loginfo("Finished gripper release")
self._step_complete(goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def move_gripper_release(self,
mode=ControllerCommander.MODE_AUTO_TRAJECTORY,
goal=None):
self._begin_step(goal)
try:
self.controller_commander.set_controller_mode(
mode, 0.8 * self.speed_scalar, [], [])
result = None
if (self.state != "plan_gripper_release"):
self.plan_gripper_release()
self.state = "gripper_release"
self.process_index = 8
self.process_starts[self.process_states[
self.process_index]] = self.get_current_pose()
path = self.plan_dictionary['gripper_release']
self._execute_path(path, goal)
except Exception as err:
traceback.print_exc()
self._step_failed(err, goal)
def _fill_process_state(self):
s = ProcessState()
s.state = self.state if self.state is not None else ""
s.payload = self.current_payload if self.current_payload is not None else ""
s.target = self.current_target if self.current_target is not None else ""
return s
def publish_process_state(self):
s = self._fill_process_state()
self._process_state_pub.publish(s)
def place_lower_temp(self):
UV = np.zeros([32, 2])
P = np.zeros([32, 3])
def _update_payload_pose(self,
payload_name,
pose,
parent_frame_id=None,
confidence=0.1):
payload = self._get_payload(payload_name)
if parent_frame_id is None:
parent_frame_id = payload.header.frame_id
parent_tf = self.tf_listener.lookupTransform(parent_frame_id,
pose.parent_frame_id,
rospy.Time(0))
pose2 = parent_tf.inv() * pose
req = UpdatePayloadPoseRequest()
req.name = payload_name
req.pose = rox_msg.transform2pose_msg(pose2)
req.header.frame_id = parent_frame_id
req.confidence = confidence
res = self.update_payload_pose_srv(req)
if not res.success:
raise Exception("Could not update payload pose")
def _get_payload(self, payload_name):
payload_array_res = self.get_payload_array_srv(
GetPayloadArrayRequest([payload_name]))
if len(payload_array_res.payload_array.payloads
) != 1 or payload_array_res.payload_array.payloads[
0].name != payload_name:
raise Exception("Invalid payload specified")
return payload_array_res.payload_array.payloads[0]
def _begin_step(self, goal):
if goal is not None:
with self._goal_handle_lock:
if self._goal_handle is not None:
res = ProcessStepResult()
res.state = self.state
res.target = self.current_target if self.current_target is not None else ""
res.payload = self.current_payload if self.current_payload is not None else ""
res.error_msg = "Attempt to execute new step while previous step running"
goal.set_rejected(result=res)
rospy.loginfo(
"Attempt to execute new step while previous step running"
)
raise Exception(
"Attempt to execute new step while previous step running"
)
else:
goal.set_accepted()
self._goal_handle = goal
def _step_complete(self, goal):
if goal is not None:
with self._goal_handle_lock:
self.publish_process_state()
res = ProcessStepResult()
res.state = self.state
res.target = self.current_target if self.current_target is not None else ""
res.payload = self.current_payload if self.current_payload is not None else ""
goal.set_succeeded(res)
if (self._goal_handle == goal):
self._goal_handle = None
def _step_failed(self, err, goal):
if goal is None:
raise err
else:
with self._goal_handle_lock:
self.publish_process_state()
res = ProcessStepResult()
res.state = self.state
res.target = self.current_target if self.current_target is not None else ""
res.payload = self.current_payload if self.current_payload is not None else ""
res.error_msg = str(err)
goal.set_aborted(result=res)
if (self._goal_handle == goal):
self._goal_handle = None
def _execute_path(self, path, goal, ft_stop=False):
if goal is None:
self.controller_commander.execute_trajectory(path, ft_stop=ft_stop)
return
def done_cb(err):
rospy.loginfo("safe_kinematic_controller generated: %s", str(err))
if (goal is not None):
if err is None:
self._step_complete(goal)
else:
with self._goal_handle_lock:
if self._goal_handle == goal:
self._goal_handle = None
res = ProcessStepResult()
res.state = self.state
res.target = self.current_target if self.current_target is not None else ""
res.payload = self.current_payload if self.current_payload is not None else ""
res.error_msg = str(err)
goal.set_aborted(result=res)
rospy.loginfo("safe_kinematic_controller generated: %s",
str(err))
with self._goal_handle_lock:
if goal.get_goal_status().status != actionlib.GoalStatus.ACTIVE:
if self._goal_handle == goal:
self._goal_handle = None
res = ProcessStepResult()
res.state = self.state
res.target = self.current_target if self.current_target is not None else ""
res.payload = self.current_payload if self.current_payload is not None else ""
res.error_msg = str(err)
goal.set_aborted(result=res)
rospy.loginfo("goal aborted before move")
return
self.controller_commander.async_execute_trajectory(path,
done_cb=done_cb,
ft_stop=ft_stop)
def _plan(self,
target_pose,
waypoints_pose=[],
speed_scalar=1,
config=None,
smoother_config=None):
error_count = 0
while True:
try:
rospy.loginfo("begin rough trajectory planning with config " +
str(config))
plan1 = self.planner.trajopt_plan(target_pose,
json_config_name=config)
rospy.loginfo("rough trajectory planning complete " +
str(config))
if smoother_config is None:
return plan1
rospy.loginfo("begin trajectory smoothing with config " +
str(smoother_config))
plan2 = self.planner.trajopt_smooth_trajectory(
plan1, json_config_name=smoother_config)
rospy.loginfo("trajectory smoothing complete with config " +
str(smoother_config))
return plan2
except:
rospy.logerr("trajectory planning failed with config " +
str(config) + " smoother_config " +
str(smoother_config))
error_count += 1
if error_count > 3:
raise
traceback.print_exc()
def main():
t1 = time.time()
do_place = "place-panel" in sys.argv
if not "disable-ft" in sys.argv:
ft_threshold1 = ft_threshold
else:
ft_threshold1 = []
rospy.init_node('Vision_MoveIt_new_Cam_wason2', anonymous=True)
print "============ Starting setup"
listener = PayloadTransformListener()
rapid_node = RAPIDCommander()
controller_commander = ControllerCommander()
object_commander = ObjectRecognitionCommander()
controller_commander.set_controller_mode(controller_commander.MODE_HALT, 1,
[], ft_threshold1)
object_target = object_commander.get_object_gripper_target_pose(
"leeward_mid_panel")
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 1, [], ft_threshold1)
print "============ Printing robot Pose"
print controller_commander.get_current_pose_msg()
#print robot.get_current_state().joint_state.position
print "============ Generating plan 1"
pose_target = copy.deepcopy(object_target)
pose_target.p[2] += 0.5
print 'Target:', pose_target
print "============ Executing plan1"
controller_commander.plan_and_move(pose_target)
print 'Execution Finished.'
########## Vertical Path 1 ############
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 0.8, [], ft_threshold1)
print "============ Printing robot Pose"
print controller_commander.get_current_pose_msg()
print "============ Generating plan 2"
pose_target2 = copy.deepcopy(object_target)
pose_target2.p[2] += 0.15
print 'Target:', pose_target2
print "============ Executing plan2"
controller_commander.compute_cartesian_path_and_move(
pose_target2, avoid_collisions=False)
print 'Execution Finished.'
########## Vertical Path 2 ############
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 0.4, [], ft_threshold1)
print "============ Printing robot Pose"
print controller_commander.get_current_pose_msg()
print "============ Generating plan 3"
pose_target2 = copy.deepcopy(object_target)
pose_target2.p[2] -= 0.15
print 'Target:', pose_target2
print "============ Executing plan3"
try:
controller_commander.compute_cartesian_path_and_move(
pose_target2, avoid_collisions=False)
pass
except:
pass
print 'Execution Finished.'
########## Lift Path ############
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [], [])
print "============ Lift panel!"
rapid_node.set_digital_io("Vacuum_enable", 1)
time.sleep(0.5)
pose_target3 = copy.deepcopy(object_target)
pose_target3.p[2] += 0.5
print 'Target:', pose_target3
print "============ Executing plan4"
controller_commander.compute_cartesian_path_and_move(
pose_target3, avoid_collisions=False)
if (do_place):
print "=========== Do place!"
print ""
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 1, [], ft_threshold1)
print "============ Generating plan 5"
time.sleep(2)
panel_target_pose = listener.lookupTransform(
"world", "panel_nest_leeward_mid_panel_target", rospy.Time(0))
panel_gripper_target_pose = listener.lookupTransform(
"leeward_mid_panel", "leeward_mid_panel_gripper_target",
rospy.Time(0))
pose_target = panel_target_pose * panel_gripper_target_pose
print pose_target.R
print pose_target.p
pose_target2 = copy.deepcopy(pose_target)
pose_target2.p[2] += 0.3
pose_target2.p[1] -= 0.2
print "============ Executing plan 5"
controller_commander.plan_and_move(pose_target2)
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 0.4, [], ft_threshold1)
print "============ Printing robot Pose"
print controller_commander.get_current_pose_msg()
print "============ Generating plan 3"
pose_target2 = copy.deepcopy(pose_target)
pose_target2.p[2] -= 0.15
print 'Target:', pose_target2
print "============ Executing plan3"
try:
controller_commander.compute_cartesian_path_and_move(
pose_target2, avoid_collisions=False)
except:
pass
print 'Execution Finished.'
print "============ Lift gripper!"
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [], [])
rapid_node.set_digital_io("Vacuum_enable", 0)
time.sleep(0.5)
pose_target3 = copy.deepcopy(pose_target)
pose_target3.p[2] += 0.5
print 'Target:', pose_target3
print "============ Executing plan4"
controller_commander.compute_cartesian_path_and_move(
pose_target3, avoid_collisions=False)
t2 = time.time()
print 'Execution Finished.'
print "Execution time: " + str(t2 - t1) + " seconds"
def __init__(self):
self.client = actionlib.SimpleActionClient("recognize_objects",
ObjectRecognitionAction)
self.listener = PayloadTransformListener()
def __init__(self, *args, **kwargs):
super(TestTesseractDiff, self).__init__(*args, **kwargs)
self.tf_listener = PayloadTransformListener()
def __init__(self, *args, **kwargs):
super(TestAttachedCollisionObject, self).__init__(*args, **kwargs)
self.tf_listener = PayloadTransformListener()
class TestAttachedCollisionObject(unittest.TestCase):
def __init__(self, *args, **kwargs):
super(TestAttachedCollisionObject, self).__init__(*args, **kwargs)
self.tf_listener = PayloadTransformListener()
def test_attached_collision_object(self):
expected_scene_1 = rospy.wait_for_message("/expected_planning_scene_1",
PlanningScene,
timeout=10)
scene_1 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_1, scene_1)
rospy.sleep(rospy.Duration(1))
update_pose_proxy = rospy.ServiceProxy("update_payload_pose",
UpdatePayloadPose)
payload2_to_gripper_target_tf = self.tf_listener.lookupTransform(
"payload2", "payload2_gripper_target", rospy.Time(0))
req = UpdatePayloadPoseRequest()
req.header.frame_id = "vacuum_gripper_tool"
req.name = "payload2"
req.pose = rox_msg.transform2pose_msg(
payload2_to_gripper_target_tf.inv())
req.confidence = 0.5
res = update_pose_proxy(req)
assert res.success
expected_scene_2 = rospy.wait_for_message("/expected_planning_scene_2",
PlanningScene,
timeout=10)
scene_2 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_2, scene_2)
world_to_fixture2_payload2_target_tf = self.tf_listener.lookupTransform(
"world", "fixture2_payload2_target", rospy.Time(0))
#Add in an offset to represent a final placement error
fixture2_payload2_target_to_payload2_tf = rox.Transform(
rox.rot([0, 0, 1], np.deg2rad(5)), [0.1, 0, 0],
"fixture2_payload2_target", "payload2")
req2 = UpdatePayloadPoseRequest()
req2.header.frame_id = "world"
req2.name = "payload2"
req2.pose = rox_msg.transform2pose_msg(
world_to_fixture2_payload2_target_tf *
fixture2_payload2_target_to_payload2_tf)
res2 = update_pose_proxy(req2)
assert res2.success
expected_scene_3 = rospy.wait_for_message("/expected_planning_scene_3",
PlanningScene,
timeout=10)
scene_3 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_3, scene_3)
def _assert_planning_scene_equal(self, scene1, scene2):
self.assertEqual(scene1.name, scene2.name)
self.assertEqual(scene1.robot_model_name, scene2.robot_model_name)
self.assertEqual(scene1.fixed_frame_transforms,
scene2.fixed_frame_transforms)
self.assertEqual(scene1.link_padding, scene2.link_padding)
self.assertEqual(scene1.link_scale, scene2.link_scale)
self.assertEqual(scene1.object_colors, scene2.object_colors)
self.assertEqual(scene1.world.collision_objects,
scene2.world.collision_objects)
self.assertEqual(scene1.is_diff, scene2.is_diff)
self.assertEqual(scene1.robot_state.is_diff,
scene2.robot_state.is_diff)
self.assertEqual(len(scene1.robot_state.attached_collision_objects),
len(scene2.robot_state.attached_collision_objects))
for i in xrange(len(scene1.robot_state.attached_collision_objects)):
self._assert_attached_collision_object_equal(
scene1.robot_state.attached_collision_objects[i],
scene2.robot_state.attached_collision_objects[i])
def _assert_attached_collision_object_equal(self, aco1, aco2):
self.assertEqual(aco1.link_name, aco2.link_name)
self._assert_collision_object_equal(aco1.object, aco2.object)
self.assertEqual(aco1.touch_links, aco2.touch_links)
self.assertEqual(aco1.detach_posture, aco2.detach_posture)
self.assertEqual(aco1.weight, aco2.weight)
def _assert_collision_object_equal(self, co1, co2):
self.assertEqual(co1.header.frame_id, co2.header.frame_id)
self.assertEqual(co1.id, co2.id)
self.assertEqual(co1.type, co2.type)
self.assertEqual(co1.primitives, co2.primitives)
self._assert_pose_almost_equal(co1.primitive_poses,
co2.primitive_poses)
for m1, m2 in zip(co1.meshes, co2.meshes):
self._assert_mesh_almost_equal(m1, m2)
self._assert_pose_almost_equal(co1.mesh_poses, co2.mesh_poses)
self.assertEqual(co1.planes, co2.planes)
self._assert_pose_almost_equal(co1.plane_poses, co2.plane_poses)
self.assertEqual(co1.operation, co2.operation)
def _assert_mesh_almost_equal(self, m1, m2):
for t1, t2 in zip(m1.triangles, m2.triangles):
np.testing.assert_equal(t1.vertex_indices, t2.vertex_indices)
for v1, v2 in zip(m1.vertices, m2.vertices):
np.testing.assert_allclose([v1.x, v1.y, v1.z], [v2.x, v2.y, v2.z])
def _assert_pose_almost_equal(self, pose1, pose2):
if isinstance(pose1, list):
for p1, p2 in zip(pose1, pose2):
self._assert_pose_almost_equal(p1, p2)
return
np.testing.assert_allclose([pose1.position.x, pose1.position.y, pose1.position.z], \
[pose2.position.x, pose2.position.y, pose2.position.z])
np.testing.assert_allclose([pose1.orientation.x, pose1.orientation.y, \
pose1.orientation.z, pose1.orientation.w], \
[pose2.orientation.x, pose2.orientation.y, \
pose2.orientation.z, pose2.orientation.w] )
class PlacementController(object):
def __init__(self):
#Subscribe to controller_state
self.controller_state_sub = rospy.Subscriber("controller_state", controllerstate, self.callback)
self.last_ros_image_stamp = rospy.Time(0)
self.goal_handle=None
self.listener = PayloadTransformListener()
self.rapid_node = rapid_node_pkg.RAPIDCommander()
self.controller_commander = controller_commander_pkg.ControllerCommander()
self.object_commander = ObjectRecognitionCommander()
# Initilialize aruco boards and parameters
self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_ARUCO_ORIGINAL)
self.parameters = cv2.aruco.DetectorParameters_create()
self.parameters.cornerRefinementMethod=cv2.aruco.CORNER_REFINE_SUBPIX
self.parameters.adaptiveThreshWinSizeMax=30
self.parameters.adaptiveThreshWinSizeStep=7
# ================== Cam 636
# --- Subscribe to Gripper camera node for image acquisition
#TODO: Take in camera names and make subscribers and triggers accordingly
self.ros_gripper_2_img_sub = rospy.Subscriber('/gripper_camera_2/image', Image, self.object_commander.ros_raw_gripper_2_image_cb)
self.ros_gripper_2_trigger = rospy.ServiceProxy('/gripper_camera_2/trigger', Trigger)
# --- Camera parameters
self.CamParam = CameraParams()
# --- Camera pose
#TODO: Substitute transform in here
R_Jcam = np.array([[0.9995,-0.0187,-0.0263],[-0.0191,-0.9997,-0.0135],[-0.0261,0.0140,-0.9996]])
r_cam = rox.hat(np.array([0.0707, 1.1395, 0.2747]))#rox.hat(np.array([- 0.2811, -1.1397,0.0335]))#rox.hat(np.array([- 0.2811, 1.1397,0.0335]))
self.R_Jcam = np.linalg.inv(np.vstack([ np.hstack([R_Jcam,np.zeros([3,3])]), np.hstack([np.dot(r_cam,R_Jcam),R_Jcam]) ]))
self.dt = None
self.iteration=0
self.board_ground = None
# functions like a gain, used with velocity to track position
self.FTdata = None
self.ft_flag = False
#self.FTdata_0 = self.FTdata
#self.FTdata_0est = self.compute_ft_est()
#self.result = self.take_image()
#TODO: replace with trajopt code
self.client = actionlib.SimpleActionClient("joint_trajectory_action", FollowJointTrajectoryAction)
# IBVS parameters
self.du_converge_TH = None
self.dv_converge_TH = None
self.iteration_limit = None
self.Ki = None
# Compliance controller parameters
self.F_d_set1 = None
self.F_d_set2 = None
self.Kc = None
self.ros_data=None
self.camMatrix=None
self.distCoeffs=None
self.ros_gripper_2_cam_info_sub=rospy.Subscriber('/gripper_camera_2/camera_info', CameraInfo, self.fill_camera_data)
def fill_camera_data(self,ros_data):
self.ros_data=ros_data
camMatrix=np.reshape(ros_data.K,(3,3))
distCoeffs=np.array([ros_data.D])
#if len(distCoeffs[0]) < 4:
# distCoeffs=np.array([[0,0,0,0,0]])
self.CamParam.distCoeff=distCoeffs
self.CamParam.camMatrix=camMatrix
def callback(self, data):
self.FTdata = np.array([data.ft_wrench.torque.x,data.ft_wrench.torque.y,data.ft_wrench.torque.z,\
data.ft_wrench.force.x,data.ft_wrench.force.y,data.ft_wrench.force.z])
self.ft_flag=True
def trapezoid_gen(self,target,current_joint_angles,acc,dx):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names=['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
goal.trajectory.header.frame_id='/world'
dist = np.array(target-current_joint_angles)
duration = np.max(np.sqrt(np.abs(9.0*dist/2.0/acc)))
vmax = 1.5/duration
amax = 3*vmax/duration
dmax = amax
[x0,v0,a0,ta,tb,tf] = trapgen(0,1,0,0,vmax,amax,dmax,0)
[xa,va,aa,ta,tb,tf] = trapgen(0,1,0,0,vmax,amax,dmax,ta)
[xb,vb,ab,ta,tb,tf] = trapgen(0,1,0,0,vmax,amax,dmax,tb)
duration = np.max(np.sqrt(np.abs(9.0*dist/2.0/acc)))
vmax = 1.5*dist/duration
acc = 3*vmax/duration
p1=JointTrajectoryPoint()
p1.positions = current_joint_angles
p1.velocities = np.zeros((6,))
p1.accelerations = aa*dist
p1.time_from_start = rospy.Duration(0)
p2=JointTrajectoryPoint()
p2.positions = np.array(p1.positions) + dist*xa
p2.velocities = va*dist
p2.accelerations = np.zeros((6,))
p2.time_from_start = rospy.Duration(ta)
p3=JointTrajectoryPoint()
p3.positions = np.array(p1.positions) + dist*xb
p3.velocities = vb*dist
p3.accelerations = -ab*dist
p3.time_from_start = rospy.Duration(tb)
p4=JointTrajectoryPoint()
p4.positions = target
p4.velocities = np.zeros((6,))
p4.accelerations = np.zeros((6,))
p4.time_from_start = rospy.Duration(tf)
goal.trajectory.points.append(p1)
goal.trajectory.points.append(p2)
goal.trajectory.points.append(p3)
goal.trajectory.points.append(p4)
return goal
def compute_ft_est(self):
Tran_z = np.array([[0,0,-1],[0,-1,0],[1,0,0]])
Vec_wrench = 100*np.array([0.019296738361905,0.056232033265447,0.088644197659430,
0.620524934626544,-0.517896661195076,0.279323567303444,-0.059640563813256,
0.631460085138371,-0.151143175570223,-6.018321330845553]).transpose()
T = self.listener.lookupTransform("base", "link_6", rospy.Time(0))
rg = 9.8*np.matmul(np.matmul(T.R,Tran_z).transpose(),np.array([0,0,1]).transpose())
A1 = np.hstack([rox.hat(rg).transpose(),np.zeros([3,1]),np.eye(3),np.zeros([3,3])])
A2 = np.hstack([np.zeros([3,3]),rg.reshape([3,1]),np.zeros([3,3]),np.eye(3)])
A = np.vstack([A1,A2])
return np.matmul(A,Vec_wrench)
def get_pose(self, board, corners, ids, CamParam):
#Define object and image points of both tags
objPoints, imgPoints = aruco.getBoardObjectAndImagePoints(board, corners, ids)
objPoints = objPoints.reshape([objPoints.shape[0],3])
imgPoints = imgPoints.reshape([imgPoints.shape[0],2])
#Get pose of both ground and panel markers from detected corners
retVal, rvec, tvec = cv2.solvePnP(objPoints, imgPoints, CamParam.camMatrix, CamParam.distCoeff)
Rca, b = cv2.Rodrigues(rvec)
return imgPoints,rvec, tvec, Rca, b
def image_jacobian_gen(self, result, corners, ids, CamParam, board_ground,board_panel,id_start_ground, id_board_ground_size, tag_ground_size, loaded_object_points_ground_in_panel_system, display_window):
rospy.loginfo(str(id_start_ground))
rospy.loginfo(str(id_board_ground_size))
rospy.loginfo(str(tag_ground_size))
rospy.loginfo(str(corners)) #float32
rospy.loginfo(str(board_ground))
rospy.loginfo(str(board_panel))
idx_max = 180
UV = np.zeros([idx_max,8])
P = np.zeros([idx_max,3])
id_valid = np.zeros([idx_max,1])
f_hat_u = CamParam.camMatrix[0][0]
f_hat_v = CamParam.camMatrix[1][1]
f_0_u = CamParam.camMatrix[0][2]
f_0_v = CamParam.camMatrix[1][2]
imgPoints_ground, rvec_ground, tvec_ground, Rca_ground, b_ground = self.get_pose(board_ground, corners, ids, CamParam)
imgPoints_panel, rvec_panel, tvec_panel, Rca_panel, b_panel = self.get_pose(board_panel, corners, ids, CamParam)
corners_ground = []
corners_panel = []
for i_ids,i_corners in zip(ids,corners):
if i_ids<=(id_start_ground+id_board_ground_size):
corners_ground.append(i_corners)
else:
corners_panel.append(i_corners)
#rospy.loginfo(str(id_start_ground))
rvec_all_markers_ground, tvec_all_markers_ground, _ = aruco.estimatePoseSingleMarkers(corners_ground, tag_ground_size, CamParam.camMatrix, CamParam.distCoeff)
rvec_all_markers_panel, tvec_all_markers_panel, _ = aruco.estimatePoseSingleMarkers(corners_panel, 0.025, CamParam.camMatrix, CamParam.distCoeff)
#rospy.loginfo(str(tvec_all_markers_ground))
#rospy.loginfo(str(tvec_all_markers_panel))
tvec_all=np.concatenate((tvec_all_markers_ground,tvec_all_markers_panel),axis=0)
for i_ids,i_corners,i_tvec in zip(ids,corners,tvec_all):
if i_ids<idx_max:
#print 'i_corners',i_corners,i_corners.reshape([1,8])
UV[i_ids,:] = i_corners.reshape([1,8]) #np.average(i_corners, axis=1)
P[i_ids,:] = i_tvec
id_valid[i_ids] = 1
id_select = range(id_start_ground,(id_start_ground+id_board_ground_size))
#used to find the height of the tags and the delta change of height, z height at desired position
Z = P[id_select,2] #- [0.68184539, 0.68560932, 0.68966803, 0.69619578])
id_valid = id_valid[id_select]
dutmp = []
dvtmp = []
#Pixel estimates of the ideal ground tag location
reprojected_imagePoints_ground_2, jacobian2 = cv2.projectPoints( loaded_object_points_ground_in_panel_system.transpose(), rvec_panel, tvec_panel, CamParam.camMatrix, CamParam.distCoeff)
reprojected_imagePoints_ground_2 = reprojected_imagePoints_ground_2.reshape([reprojected_imagePoints_ground_2.shape[0],2])
if(display_window):
frame_with_markers_and_axis = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR)
frame_with_markers_and_axis = cv2.aruco.drawAxis( frame_with_markers_and_axis, CamParam.camMatrix, CamParam.distCoeff, Rca_ground, tvec_ground, 0.2 )
frame_with_markers_and_axis = cv2.aruco.drawAxis( frame_with_markers_and_axis, CamParam.camMatrix, CamParam.distCoeff, Rca_panel, tvec_panel, 0.2 )
#plot image points for ground tag from corner detection and from re-projections
for point1,point2 in zip(imgPoints_ground,np.float32(reprojected_imagePoints_ground_2)):
cv2.circle(frame_with_markers_and_axis,tuple(point1),5,(0,0,255),3)
cv2.circle(frame_with_markers_and_axis,tuple(point2),5,(255,0,0),3)
height, width, channels = frame_with_markers_and_axis.shape
cv2.imshow(display_window,cv2.resize(frame_with_markers_and_axis, (width/4, height/4)))
cv2.waitKey(1)
#Save
#filename_image = "/home/rpi-cats/Desktop/DJ/Code/Images/Panel2_Acquisition_"+str(t1)+"_"+str(iteration)+".jpg"
#scipy.misc.imsave(filename_image, frame_with_markers_and_axis)
reprojected_imagePoints_ground_2 = np.reshape(reprojected_imagePoints_ground_2,(id_board_ground_size,8))
#Go through a particular point in all tags to build the complete Jacobian
for ic in range(4):
#uses first set of tags, numbers used to offset camera frame, come from camera parameters
UV_target = np.vstack([reprojected_imagePoints_ground_2[:,2*ic]-f_0_u, reprojected_imagePoints_ground_2[:,2*ic+1]-f_0_v]).T
uc = UV_target[:,0]
vc = UV_target[:,1]
UV_current = np.vstack([UV[id_select,2*ic]-f_0_u, UV[id_select,2*ic+1]-f_0_v]).T
#find difference between current and desired tag difference
delta_UV = UV_target-UV_current
delet_idx = []
J_cam_tmp =np.array([])
for tag_i in range(id_board_ground_size):
if id_valid[tag_i] == 1:
tmp = 1.0*np.array([[uc[tag_i]*vc[tag_i]/f_hat_u, -1.0*(uc[tag_i]*uc[tag_i]/f_hat_u + f_hat_u), vc[tag_i],-f_hat_u/Z[tag_i], 0.0, uc[tag_i]/Z[tag_i]],
[ vc[tag_i]*vc[tag_i]/f_hat_v+f_hat_v, -1.0*uc[tag_i]*vc[tag_i]/f_hat_v, -uc[tag_i],0.0, -f_hat_v/Z[tag_i], vc[tag_i]/Z[tag_i]]])
if not (J_cam_tmp).any():
J_cam_tmp = tmp
else:
J_cam_tmp= np.concatenate((J_cam_tmp, tmp), axis=0)
else:
delet_idx.append(tag_i)
delta_UV = np.delete(delta_UV, delet_idx, 0)
dutmp.append(np.mean(delta_UV[:,0]))
dvtmp.append(np.mean(delta_UV[:,1]))
#camera jacobian
if ic ==0:
J_cam = J_cam_tmp
delta_UV_all = delta_UV.reshape(np.shape(delta_UV)[0]*np.shape(delta_UV)[1],1)
UV_target_all = UV_target.reshape(np.shape(UV_target)[0]*np.shape(UV_target)[1],1)
else:
J_cam = np.vstack([J_cam,J_cam_tmp])
delta_UV_all = np.vstack([delta_UV_all,delta_UV.reshape(np.shape(delta_UV)[0]*np.shape(delta_UV)[1],1)])
UV_target_all = np.vstack([UV_target_all,UV_target.reshape(np.shape(UV_target)[0]*np.shape(UV_target)[1],1)])
du = np.mean(np.absolute(dutmp))
dv = np.mean(np.absolute(dvtmp))
print 'Average du of all points:',du
print 'Average dv of all points:',dv
return du, dv, J_cam, delta_UV_all
def take_image(self):
####################### SET INITIAL POSE BASED ON PBVS #######################
#### Final Nest Placement Error Calculation ===============================
#Read new image
self.last_ros_image_stamp = self.object_commander.ros_image_stamp
try:
self.ros_gripper_2_trigger.wait_for_service(timeout=0.1)
self.ros_gripper_2_trigger()
except:
pass
wait_count=0
while self.object_commander.ros_image is None or self.object_commander.ros_image_stamp == self.last_ros_image_stamp:
if wait_count > 50:
raise Exception("Image receive timeout")
time.sleep(0.25)
wait_count += 1
self.result = self.object_commander.ros_image
def move_to_initial_pose(self):
#Set controller command mode
self.controller_commander.set_controller_mode(self.controller_commander.MODE_AUTO_TRAJECTORY, 0.4, [],[])
time.sleep(0.5)
#open loop set the initial pose to the end pose in the transport path
#pose_target2 = rox.Transform(rot0, tran0)
##Execute movement to set location
rospy.loginfo("Executing initial path ====================")
#TODO: Change to trajopt planning
self.controller_commander.compute_cartesian_path_and_move(self.initial_pose, avoid_collisions=False)
def pbvs_to_stage1(self,with_lower):
self.controller_commander.set_controller_mode(self.controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [])
tvec_err = np.array([1000,1000,1000])
while(np.linalg.norm(tvec_err) > 0.06):
self.take_image()
#Detect tag corners in aqcuired image using aruco
corners, ids, _ = cv2.aruco.detectMarkers(self.result, self.aruco_dict, parameters=self.parameters)
#Sort corners and ids according to ascending order of ids
#rospy.loginfo("tvec difference: %f, %f",corners,ids)
corners, ids = sort_corners(corners,ids)
# Estimate Poses
imgPoints_ground,rvec_ground, tvec_ground, Rca_ground, b_ground = self.get_pose(self.board_ground, corners, ids, self.CamParam)
imgPoints_panel,rvec_panel, tvec_panel, Rca_panel, b_panel = self.get_pose(self.board_panel, corners, ids, self.CamParam)
rospy.loginfo("camera params")
rospy.loginfo(str(self.CamParam.distCoeff))
rospy.loginfo(str(self.CamParam.camMatrix))
observed_tvec_difference = tvec_ground-tvec_panel
observed_rvec_difference = rvec_ground-rvec_panel
tvec_err = self.loaded_tvec_difference_stage1-observed_tvec_difference
rvec_err = self.loaded_rvec_difference_stage1-observed_rvec_difference
rospy.loginfo("tvec difference: %f, %f, %f",tvec_err[0],tvec_err[1],tvec_err[2])
rospy.loginfo("rvec difference: %f, %f, %f",rvec_err[0],rvec_err[1],rvec_err[2])
if(not with_lower):
tvec_err[2] = 0
else:
tvec_err[2] -= 0.04
dx = np.array([0,0,0, -tvec_err[0], tvec_err[1],tvec_err[2]])*0.7
#dx = np.array([rvec_err[0],rvec_err[1],rvec_err[2], -tvec_err[0], tvec_err[1],tvec_err[2]])*0.7
# Adjustment
rospy.loginfo("PBVS to initial Position ====================")
current_joint_angles = self.controller_commander.get_current_joint_values()
print dx
joints_vel = QP_abbirb6640(np.array(current_joint_angles).reshape(6, 1),np.array(dx))
goal = self.trapezoid_gen(np.array(current_joint_angles) + joints_vel.dot(1),np.array(current_joint_angles),0.25,np.array(dx))
#TODO replace with trajopt code
self.client.wait_for_server()
self.client.send_goal(goal)
self.client.wait_for_result()
res = self.client.get_result()
if (res.error_code != 0):
raise Exception("Trajectory execution returned error")
rospy.loginfo("End of Initial Pose ====================")
### End of initial pose
def final_adjustment(self):
############################## FINAL ADJUSTMENT ##############################
rospy.loginfo("Final Adjustment")
self.controller_commander.set_controller_mode(self.controller_commander.MODE_AUTO_TRAJECTORY, 0.2, [],[])
self.take_image()
#Detect tag corners in aqcuired image using aruco
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(self.result, self.aruco_dict, parameters=self.parameters)
#Sort corners and ids according to ascending order of ids
corners, ids = sort_corners(corners,ids)
# Estimate Poses
imgPoints_ground, rvec_ground, tvec_ground, Rca_ground, b_ground = self.get_pose(self.board_ground, corners, ids, self.CamParam)
imgPoints_panel, rvec_panel, tvec_panel, Rca_panel, b_panel = self.get_pose(self.board_panel, corners, ids, self.CamParam)
observed_tvec_difference = tvec_ground-tvec_panel
observed_rvec_difference = rvec_ground-rvec_panel
rospy.loginfo("============== Difference in nest position (before adjusment)")
tvec_err = self.loaded_tvec_difference-observed_tvec_difference
rvec_err = self.loaded_rvec_difference-observed_rvec_difference
rospy.loginfo("tvec difference: %f, %f, %f",tvec_err[0],tvec_err[1],tvec_err[2])
rospy.loginfo("rvec difference: %f, %f, %f",rvec_err[0],rvec_err[1],rvec_err[2])
current_joint_angles = self.controller_commander.get_current_joint_values()
dx = np.array([0,0,0, -tvec_err[0], tvec_err[1],0])
joints_vel = QP_abbirb6640(np.array(current_joint_angles).reshape(6, 1),np.array(dx))
goal = self.trapezoid_gen(np.array(current_joint_angles) + joints_vel.dot(1),np.array(current_joint_angles),0.25,np.array(dx))
#TODO replace with trajopt code
self.client.wait_for_server()
self.client.send_goal(goal)
self.client.wait_for_result()
res = self.client.get_result()
if (res.error_code != 0):
raise Exception("Trajectory execution returned error")
self.take_image()
#Detect tag corners in aqcuired image using aruco
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(self.result, self.aruco_dict, parameters=self.parameters)
#Sort corners and ids according to ascending order of ids
corners, ids = sort_corners(corners,ids)
# Estimate Poses
imgPoints_ground, rvec_ground, tvec_ground, Rca_ground, b_ground = self.get_pose(self.board_ground, corners, ids, self.CamParam)
imgPoints_panel, rvec_panel, tvec_panel, Rca_panel, b_panel = self.get_pose(self.board_panel, corners, ids, self.CamParam)
observed_tvec_difference = tvec_ground-tvec_panel
observed_rvec_difference = rvec_ground-rvec_panel
rospy.loginfo("============== Difference in nest position (after adjusment)")
tvec_err = self.loaded_tvec_difference-observed_tvec_difference
rvec_err = self.loaded_rvec_difference-observed_rvec_difference
rospy.loginfo("tvec difference: %f, %f, %f",tvec_err[0],tvec_err[1],tvec_err[2])
rospy.loginfo("rvec difference: %f, %f, %f",rvec_err[0],rvec_err[1],rvec_err[2])
def release_suction_cups(self):
################## RELEASE SUCTION CUPS AND LIFT THE GRIPPER ##################
rospy.loginfo("============ Release Suction Cups...")
self.controller_commander.set_controller_mode(self.controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [])
self.rapid_node.set_digital_io("Vacuum_enable", 0)
#g = ProcessStepGoal('plan_place_set_second_step',"")
#process_client.send_goal(g)
time.sleep(0.5)
Cur_Pose = self.controller_commander.get_current_pose_msg()
rot_current = [Cur_Pose.pose.orientation.w, Cur_Pose.pose.orientation.x,Cur_Pose.pose.orientation.y,Cur_Pose.pose.orientation.z]
trans_current = [Cur_Pose.pose.position.x,Cur_Pose.pose.position.y,Cur_Pose.pose.position.z]
pose_target2 = rox.Transform(rox.q2R([rot_current[0], rot_current[1], rot_current[2], rot_current[3]]), trans_current)
pose_target2.p[2] += 0.25
rospy.loginfo("============ Lift gripper...")
#TODO: Change to trajopt planning
self.controller_commander.compute_cartesian_path_and_move(pose_target2, avoid_collisions=False)
#s=ProcessState()
#s.state="place_set"
#s.payload="leeward_tip_panel"
#s.target=""
#process_state_pub.publish(s)
def ibvs_placement(self):
# INPUT:
# Raw image from gripper camera (Cam#636)
# Robot state
# Panel ID -
# (1) Desired placement calibration results
# (2) Initial position
# (3) Board info
# F/T sensor data
# Camera info
# OUTPUT:
# Sequential robot joint command to achieve the desired results
# PARAMETERS:
# IBVS -
# (1) Convergence threshold (du_converge_TH, dv_converge_TH, iteration_limit)
# (2) Image Jacobian Gain (Ki)
# Complaince controller:
# (1) Desired force set points (F_d_set1, F_d_set2)
# (2) Complaince control gain (Kc)
#test = 0
################################# BEGIN IBVS #################################
self.controller_commander.set_controller_mode(self.controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [])
step_size_min = self.step_size_min
loaded_object_points_ground_in_panel_system = self.loaded_object_points_ground_in_panel_system_stage_2
du = 100.0
dv = 100.0
self.FTdata_0 = self.FTdata
self.FTdata_0est = self.compute_ft_est()
while ((du>self.du_converge_TH) | (dv>self.dv_converge_TH) and (self.iteration<self.iteration_limit)):
#try changing du and dv to lower values(number of iterations increases)
self.iteration += 1
#Print current robot pose at the beginning of this iteration
Cur_Pose = self.controller_commander.get_current_pose_msg()
self.take_image()
#Detect tag corners in aqcuired image using aruco
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(self.result, self.aruco_dict, parameters=self.parameters)
#Sort corners and ids according to ascending order of ids
corners, ids = sort_corners(corners,ids)
#check if all tags detected
if len(ids) >10:#retVal_ground != 0 and retVal_panel !=0:
du, dv, J_cam, delta_UV_all = self.image_jacobian_gen(self.result, corners, ids, self.CamParam, self.board_ground,self.board_panel,self.id_start_ground,
self.id_board_row_ground*self.id_board_col_ground,self.tag_ground_size,loaded_object_points_ground_in_panel_system,'Cam0')
dx = QP_Cam(np.dot(J_cam, self.R_Jcam),self.Ki*delta_UV_all)
dx = dx.reshape([6,1])
# Compliance Force Control
FTdata_est = self.compute_ft_est()
if(not self.ft_flag):
raise Exception("havent reached callback")
FTread = self.FTdata - self.FTdata_0 - FTdata_est + self.FTdata_0est
print '=====================FT============:',FTread[-1], self.FTdata[-1], self.FTdata_0[-1], FTdata_est[-1], self.FTdata_0est[-1]
if FTread[-1]> (self.F_d_set1+50):
F_d = self.F_d_set1
else:
F_d = self.F_d_set2
if (self.FTdata==0).all():
rospy.loginfo("FT data overflow")
else:
Vz = self.Kc*(F_d - FTread[-1])
dx[-1] = dx[-1]+Vz
current_joint_angles = self.controller_commander.get_current_joint_values()
step_size_tmp = np.linalg.norm(dx)
if step_size_tmp <= step_size_min:
step_size_min = step_size_tmp
else:
dx = dx/step_size_tmp*step_size_min
joints_vel = QP_abbirb6640(np.array(current_joint_angles).reshape(6, 1),np.array(dx))
goal = self.trapezoid_gen(np.array(current_joint_angles) + joints_vel.dot(self.dt),np.array(current_joint_angles),0.25,np.array(dx))
#TODO: replace with trajopt code
self.client.wait_for_server()
self.client.send_goal(goal)
self.client.wait_for_result()
res = self.client.get_result()
if (res.error_code != 0):
raise Exception("Trajectory execution returned error")
rospy.loginfo('Current Iteration Finished.')
rospy.loginfo("============= iteration =============")
rospy.loginfo("iteration: %f",self.iteration)
def pointarray_to_array(self,data):
arr=np.zeros((3,len(data)))
#arr_column=np.empty((3,0))
for i in range(len(data)):
#arr_column[0]=data[i].x
#arr_column[1]=data[i].y
#arr_column[2]=data[i].z
arr_column=np.array([[data[i].x],[data[i].y],[data[i].z]])
#rospy.loginfo(str(arr_column))
arr[:,i]=arr_column.reshape([3,])
#rospy.loginfo(str(arr))
#return np.asmatrix(arr,float)
rospy.loginfo(str(arr))
return arr
def vector3_to_array(self,data):
arr =np.asarray( [[data.x],[data.y],[data.z]])
'''
arr=np.empty(3)
arr[0]=data.x
arr[1]=data.y
arr[2]=data.z
arr=[]
arr.append(float(data.x))
arr.append(float(data.y))
arr.append(float(data.z))
'''
rospy.loginfo(str(arr))
#arr.reshape([3,1])
rospy.loginfo(str(arr))
return arr
#NOT FUNCTIONING
def aruco_dicts(self,aruco_dict_str):
aruco_dicts=set()
aruco_dicts.add(aruco_dict_str)
if not hasattr(cv2.aruco, next(iter(aruco_dicts))):
raise ValueError("Invalid aruco-dict value")
aruco_dict_id=getattr(cv2.aruco, next(iter(aruco_dicts)))
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id)
return aruco_dict
def single_camera_placement(self,data,camera_1_ground,camera_1_place):
try:
self.dt = data.ibvs_parameters.IBVSdt
self.iteration=0
#aruco_dict_panel=self.aruco_dicts(camera_1_place.dictionary)
#aruco_dict_ground=self.aruco_dicts(camera_1_ground.dictionary)
aruco_dict_panel = self.aruco_dicts(camera_1_place.dictionary)
aruco_dict_ground = self.aruco_dicts(camera_1_ground.dictionary)
self.board_panel = cv2.aruco.GridBoard_create(camera_1_place.markersX, camera_1_place.markersY, camera_1_place.markerLength, camera_1_place.markerSpacing, aruco_dict_panel, camera_1_place.firstMarker)
self.board_ground = cv2.aruco.GridBoard_create(camera_1_ground.markersX, camera_1_ground.markersY, camera_1_ground.markerLength, camera_1_ground.markerSpacing, aruco_dict_ground, camera_1_ground.firstMarker)
self.id_start_ground=camera_1_ground.firstMarker
self.id_board_row_ground=camera_1_ground.markersX
self.id_board_col_ground=camera_1_ground.markersY
self.tag_ground_size=camera_1_ground.markerLength
self.loaded_object_points_ground_in_panel_system_stage_2 = self.pointarray_to_array(data.point_difference_stage2)
# --- Load ideal pose difference information from file
self.loaded_rvec_difference_stage1 = self.vector3_to_array(data.rvec_difference_stage1)
self.loaded_tvec_difference_stage1 = self.vector3_to_array(data.tvec_difference_stage1)
self.loaded_tvec_difference_stage1[1]+=0.03
# --- Load ideal pose difference information from file
self.loaded_rvec_difference = self.vector3_to_array(data.rvec_difference_stage2)
self.loaded_tvec_difference = self.vector3_to_array(data.tvec_difference_stage2)
self.du_converge_TH = data.ibvs_parameters.du_converge_th
self.dv_converge_TH = data.ibvs_parameters.dv_converge_th
self.iteration_limit = data.ibvs_parameters.iteration_limit
self.Ki = data.ibvs_parameters.Ki
# Compliance controller parameters
self.F_d_set1 = data.compliance_control_parameters.F_d_set1
self.F_d_set2 = data.compliance_control_parameters.F_d_set2
self.Kc = data.compliance_control_parameters.Kc
self.initial_pose=data.initial
#self.tran0 = np.array([2.15484,1.21372,0.25766])
#self.rot0 = rox.q2R([0.02110, -0.03317, 0.99922, -0.00468])
self.step_size_min = data.ibvs_parameters.step_size_min
except Exception as err:
rospy.loginfo("Input values for placement controller are invalid "+str(err))
#feedback=PlacementStepFeedback()
traceback.print_exc()
res = PlacementStepResult()
res.state="Error"
res.error_msg=str(err)
#feedback.error_msg=
#self.goal_handle.publish_feedback(feedback)
self.goal_handle.set_aborted(str(err))
try:
#self.move_to_initial_pose()
self.pbvs_to_stage1(False)
self.pbvs_to_stage1(True)
self.ibvs_placement()
self.final_adjustment()
#self.release_suction_cups()
except Exception as err:
traceback.print_exc()
rospy.loginfo("Placement controller failed with error: "+str(err))
#feedback=PlacementStepFeedback()
res = PlacementStepResult()
res.state="Error"
res.error_msg=str(err)
self.goal_handle.set_aborted(res)
#res = PlacementStepResult()
#res.state="Placement_complete"
#res.error_msg=""
self.goal_handle.set_succeeded(None)
def two_camera_placement(self,data,camera_1_ground,camera_1_place,camera_2_ground,camera_2_place):
self.dt = data.ibvs_parameters.IBVSdt
self.iteration=0
self.board_panel = cv2.aruco.GridBoard_create(camera_1_place.markersX, camera_1_place.markersY, camera_1_place.markerLength, camera_1_place.markerSpacing, camera_1_place.dictionary, camera_1_place.firstMarker)
self.board_ground = cv2.aruco.GridBoard_create(camera_1_ground.markersX, camera_1_ground.markersY, camera_1_ground.markerLength, camera_1_ground.markerSpacing, camera_1_ground.dictionary, camera_1_ground.firstMarker)
self.board_panel2 = cv2.aruco.GridBoard_create(camera_2_place.markersX, camera_2_place.markersY, camera_2_place.markerLength, camera_2_place.markerSpacing, camera_2_place.dictionary, camera_2_place.firstMarker)
self.board_ground2 = cv2.aruco.GridBoard_create(camera_2_ground.markersX, camera_2_ground.markersY, camera_2_ground.markerLength, camera_2_ground.markerSpacing, camera_2_ground.dictionary, camera_2_ground.firstMarker)
self.loaded_object_points_ground_in_panel_system_stage_2 = self.pointarray_to_array(data.point_difference_stage2)
# --- Load ideal pose differnece information from file
self.loaded_rvec_difference_stage1 = self.vector3_to_array(data.rvec_difference_stage1)
self.loaded_tvec_difference_stage1 = self.vector3_to_array(data.tvec_difference_stage1)
self.loaded_tvec_difference_stage1[1]+=0.03
# --- Load ideal pose differnece information from file
self.loaded_rvec_difference = self.vector3_to_array(data.rvec_difference_stage2)
self.loaded_tvec_difference = self.vector3_to_array(data.tvec_difference_stage2)
self.du_converge_TH = data.ibvs_parameters.du_converge_th
self.dv_converge_TH = data.ibvs_parameters.dv_converge_th
self.iteration_limit = data.ibvs_parameters.iteration_limit
self.Ki = data.ibvs_parameters.Ki
# Compliance controller parameters
self.F_d_set1 = data.compliance_control_parameters.F_d_set1
self.F_d_set2 = data.compliance_control_parameters.F_d_set2
self.Kc = data.compliance_control_parameters.Kc
self.initial_pose=data.initial
#self.tran0 = np.array([2.15484,1.21372,0.25766])
#self.rot0 = rox.q2R([0.02110, -0.03317, 0.99922, -0.00468])
self.step_size_min = data.ibvs_parameters.step_size_min
self.move_to_initial_pose()
#self.pbvs_to_stage1()
#self.ibvs_placement()
self.final_adjustment()
self.release_suction_cups()
#res = ProcessStepResult()
#res.state="Placement_complete"
self.goal_handle.set_succeeded(None)