# modify it under the terms of the GNU Lesser General Public License # as published by the Free Software Foundation, either version 3 of # the License, or (at your option) any later version. # # sot-motion-planner is distributed in the hope that it will be useful, but # WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU # General Lesser Public License for more details. You should have # received a copy of the GNU Lesser General Public License along with # sot-motion-planner. If not, see <http://www.gnu.org/licenses/>. from dynamic_graph import plug from dynamic_graph.sot.motion_planner import Localizer l = Localizer("localizer") l.add_landmark_observation("obs") l.add_landmark_observation("obs2") # Feature space is geometrical space without the orientation. # Here we would have some kind of "global" detector # returning (x,y) world coordinate of some objects. JfeatureReferencePosition = ((1, 0, 0), (0, 1, 0)) # Robot is sensor, so jacobian is identity. JsensorPosition = ((1, 0, 0), (0, 1, 0), (0, 0, 1)) # First observation. l.obs_JfeatureReferencePosition.value = JfeatureReferencePosition l.obs_JsensorPosition.value = JsensorPosition
c1_M_c = np.matrix( [[ 0., 0., 1., 0.], [-1., 0., 0., 0.], [ 0., -1., 0., 0.], [ 0., 0., 0., 1.]]) g_M_c = matrixToTuple(g_M_c1 * c1_M_c) plug(robot.dynamic.gaze, w_M_c.positionIN) plug(robot.dynamic.Jgaze, w_M_c.jacobianIN) w_M_c.setTransformation(g_M_c) # Localization l = Localizer('localizer') sensors = [w_M_c] f = 1. (px, py) = (1., 1.) (u0, v0) = (0., 0.) C = np.matrix( [[ f * px, 0., u0 ], [ 0., f * py, v0 ], [ 0., 0., 1. ]], dtype = np.dtype(np.float) ) def S(q, sensorId):
# modify it under the terms of the GNU Lesser General Public License # as published by the Free Software Foundation, either version 3 of # the License, or (at your option) any later version. # # sot-motion-planner is distributed in the hope that it will be useful, but # WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU # General Lesser Public License for more details. You should have # received a copy of the GNU Lesser General Public License along with # sot-motion-planner. If not, see <http://www.gnu.org/licenses/>. from dynamic_graph import plug from dynamic_graph.sot.motion_planner import Localizer l = Localizer('localizer') l.add_landmark_observation('obs') # Feature space is geometrical space without the orientation. # Here we would have some kind of "global" detector # returning (x,y) world coordinate of some objects. l.obs_JfeatureReferencePosition.value = ( (1, 0, 0), (0, 1, 0) ) # Robot is sensor, so jacobian is identity. l.obs_JsensorPosition.value = ( (1, 0, 0), (0, 1, 0), (0, 0, 1),
# # . # / \ # | x+ # y+ <--X # # (0, 0) = O = robot position # (r, 0) = sensor position # # Q = (x, y, theta) # S = (x, y) # IM = D # l = Localizer('localizer') r = 1. # robot radius error = (1, 0, 0) # dx, dy, dtheta expectedRobot = (0, 0, 0) # x, y, theta realRobot = tuple(map(lambda (p, e): p + e, zip(expectedRobot, error))) # x, y, theta # Sensor positions around the robot: # # 1 # 3 2 # . # sensorPos = (0., pi / 2.)