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