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