def calcT(self, q):
q = pin.normalize(robot.model, q)
Tg = self.reachTool.calcDiff(q) \
+ self.reachLL.calcDiff(q)*10 \
+ self.reachRL.calcDiff(q)*10 \
+ self.posture.calcDiff(q)*1e-2
return Tg
def _sample_state(self) -> Tuple[np.ndarray, np.ndarray]:
""" TODO: Write documentation.
"""
# Add noise on top of neutral configuration
qpos = self._neutral()
qpos += sample(scale=0.1, shape=(self.robot.nq,), rg=self.rg)
qpos = normalize(self.robot.pinocchio_model, qpos)
# Make sure it does not go through the ground
update_quantities(self.robot, qpos, use_theoretical_model=False)
dist_rlt = self.robot.collision_data.distanceResults
qpos[2] -= min(0.0, *[dist_req.min_distance for dist_req in dist_rlt])
# Zero mean normally distributed initial velocity
qvel = sample(
dist='normal', scale=0.1, shape=(self.robot.nv,), rg=self.rg)
return qpos, qvel
def test_basic(self):
model = self.model
q_ones = np.ones((model.nq))
self.assertFalse(pin.isNormalized(model, q_ones))
self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
q_rand = np.random.rand((model.nq))
q_rand = pin.normalize(model, q_rand)
self.assertTrue(pin.isNormalized(model, q_rand))
self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.) <= 1e-8)
q_next = pin.integrate(model, self.q, np.zeros((model.nv)))
self.assertApprox(q_next, self.q)
v_diff = pin.difference(model, self.q, q_next)
self.assertApprox(v_diff, np.zeros((model.nv)))
q_next = pin.integrate(model, self.q, self.v)
q_int = pin.interpolate(model, self.q, q_next, 0.5)
self.assertApprox(q_int, q_int)
value = pin.squaredDistance(model, self.q, self.q)
self.assertTrue((value <= 1e-8).all())
dist = pin.distance(model, self.q, self.q)
self.assertTrue(dist <= 1e-8)
q_neutral = pin.neutral(model)
self.assertApprox(q_neutral, q_neutral)
q_rand1 = pin.randomConfiguration(model)
q_rand2 = pin.randomConfiguration(model, -np.ones((model.nq)),
np.ones((model.nq)))
self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8))
self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2,
1e-8))
def callback(self, q):
q = pin.normalize(robot.model, q)
self.reachTool.callback(q)
def calc(self, q):
q = pin.normalize(robot.model, q)
return self.reachTool.calc(q) \
+ self.reachLL.calc(q)*10 \
+ self.reachRL.calc(q)*10 \
+ self.posture.calc(q)*1e-2
def callback(self, q):
q = pin.normalize(robot.model, q)
costReaching.callback(q)
def calc(self, q):
q = pin.normalize(robot.model, q)
costReaching.dim = 3
return costReaching.calc(q) + 1e-1 * costWeightedGrav.calc(q)
else:
return np.array(fs)
# Tdiffq is used to compute the tangent application in the configuration space.
Tdiffq = lambda f, q: Tdiff1(f, lambda q, v: pin.integrate(robot.model, q, v),
robot.model.nv, q)
### Num diff checking, for each cost.
q = pin.randomConfiguration(robot.model)
costReaching.dim = 6
Tg6 = costReaching.calcDiff6d(q)
g6n = numdiff(costReaching.calc6d,
q,
normalize=lambda q: pin.normalize(robot.model, q))
Tg6n = Tdiffq(costReaching.calc6d, q)
from dexp import dExpQ_inv
g6 = Tg6 @ dExpQ_inv(robot.model, q)
assert (norm(Tg6 - Tg6n) < 1e-4)
assert (norm(g6 - g6n) < 1e-4)
costReaching.dim = 3
Tg3 = costReaching.calcDiff3d(q)
g3n = numdiff(costReaching.calc3d,
q,
normalize=lambda q: pin.normalize(robot.model, q))
g3 = Tg3 @ dExpQ_inv(robot.model, q)
assert (norm(g3 - g3n) < 1e-4)
Tg = costReaching.calcDiff3d(q)
Tgn = Tdiffq(costReaching.calc3d, q)