# 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
l.obs_weight.value = (0.0, 1.0)
np.array([0.025+10. , 0. , 1.2967+3., 1.], dtype=np.float),
# (-4, -5)
np.array([0.025+10. , 0.+4. , 1.2967+5., 1.], dtype=np.float),
]
# Localizer setup.
plannedRobot = [0., 0., 0., 1.] # robot planned position
error = np.array([0., 0., 1.]) # x, y, theta
Tmax = 100
realRobot = np.inner(XYThetaToHomogeneousMatrix(error), plannedRobot)
for i in xrange(len(landmarks)):
l.add_landmark_observation('obs'+str(i))
for t in xrange(Tmax):
T = t + 10
print("--- T =", T, "---")
cfg = list(robot.dynamic.position.value)
#cfg[0] = realRobot[0]; cfg[1] = realRobot[1]; cfg[5] = realRobot[2]
cfg[0] = plannedRobot[0]; cfg[1] = plannedRobot[1]; cfg[5] = plannedRobot[2]
robot.dynamic.position.value = tuple(cfg)
robot.dynamic.gaze.recompute(T)
robot.dynamic.Jgaze.recompute(T)
w_M_c.position.recompute(T)
w_M_c.jacobian.recompute(T)
print("Planned robot position:", plannedRobot[0:3])
print("Real robot position:", realRobot[0:3])
# 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),
)
def P((x, y), l):
(xr, yr) = features[l]
return (.5 * (xr - x) * (xr - x) + .5 * (yr - y) * (yr - y),)
def dS((x, y, theta), sensor):
return ((1, 0, r * cos(sensorPos[sensor] + theta)),
(0, 1, r * sin(sensorPos[sensor] + theta)))
def dP((x, y), l):
(xr, yr) = features[l]
return ((x - xr, y - yr,), )
for sensor in xrange(len(sensorPos)):
for i in xrange(len(features)):
prefix = 'obs_{0}_{1}'.format(sensor, i)
l.add_landmark_observation(prefix)
l.signal(prefix + '_JfeatureReferencePosition').value = dP(S(expectedRobot, sensor), i)
l.signal(prefix + '_JsensorPosition').value = dS(expectedRobot, sensor)
l.signal(prefix + '_weight').value = (1.,)
l.signal(prefix + '_featureObservedPosition').value = P(S(realRobot, sensor), i)
l.signal(prefix + '_featureReferencePosition').value = \
P(S(expectedRobot, sensor), i)
l.signal(prefix + '_correctedDofs').value = \
(1., 1., 1.)
l.configurationOffset.recompute(0)
