def update_geometric(self, pose):
r"""
:param pose: current pose of the body, relative to the ground
:type pose: (4,4)-array
- g: ground body
- p: parent body
- c: child body
- r: reference frame of the joint, rigidly fixed to the parent body
- n: new frame of the joint, rigidly fixed to the child body
so H_nc and H_pr are constant.
.. math::
H_gc &= H_{gp} \ H_{pc} \\
&= H_{gp} \ (H_{pr} \ H_{rn} \ H_{nc})
"""
self._pose = pose
H_gp = pose
for j in self.childrenjoints:
H_cn = j.frame1.bpose
H_pr = j.frame0.bpose
H_rn = j.pose
H_pc = dot(H_pr, dot(H_rn, Hg.inv(H_cn)))
child_pose = dot(H_gp, H_pc)
j.frame1.body.update_geometric(child_pose)
def update_geometric(self, pose):
r"""
:param pose: current pose of the body, relative to the ground
:type pose: (4,4)-array
- g: ground body
- p: parent body
- c: child body
- r: reference frame of the joint, rigidly fixed to the parent body
- n: new frame of the joint, rigidly fixed to the child body
so H_nc and H_pr are constant.
.. math::
H_gc &= H_{gp} \ H_{pc} \\
&= H_{gp} \ (H_{pr} \ H_{rn} \ H_{nc})
"""
self._pose = pose
H_gp = pose
for j in self.childrenjoints:
H_cn = j.frame1.bpose
H_pr = j.frame0.bpose
H_rn = j.pose
H_pc = dot(H_pr, dot(H_rn, Hg.inv(H_cn)))
child_pose = dot(H_gp, H_pc)
j.frame1.body.update_geometric(child_pose)
def add_body(name, mass, com_position, gyration_radius):
#mass matrix at com
mass_g = mass * diag(hstack((gyration_radius**2, (1, 1, 1))))
H_fg = eye(4)
H_fg[0:3, 3] = com_position
H_gf = Hg.inv(H_fg)
#mass matrix at body's frame origin:
mass_o = dot(Hg.adjoint(H_gf).T, dot(mass_g, Hg.adjoint(H_gf)))
bodies.append(Body(name=prefix+name, mass=mass_o))
def update(self, dt):
r"""
This method calls the collision solver and updates the constraint
status and the contact frames poses accordingly.
The two frames have the same orientation, with the contact normal along
the `z`-axis
"""
(sdist, H_gc0, H_gc1) = self._collision_solver(self._shapes)
H_b0g = Hg.inv(self._shapes[0].frame.body.pose)
H_b1g = Hg.inv(self._shapes[1].frame.body.pose)
self._frames[0].bpose = dot(H_b0g, H_gc0)
self._frames[1].bpose = dot(H_b1g, H_gc1)
H_c0c1 = dot(Hg.inv(H_gc0), H_gc1)
dsdist = (dot(Hg.adjoint(H_c0c1)[5, :], self._frames[1].twist) -
self._frames[0].twist[5])
self._is_active = (sdist + dsdist * dt < self._proximity)
self._sdist = sdist
self._force[:] = 0.
def update(self, dt):
r"""
This method calls the collision solver and updates the constraint
status and the contact frames poses accordingly.
The two frames have the same orientation, with the contact normal along
the `z`-axis
"""
(sdist, H_gc0, H_gc1) = self._collision_solver(self._shapes)
H_b0g = Hg.inv(self._shapes[0].frame.body.pose)
H_b1g = Hg.inv(self._shapes[1].frame.body.pose)
self._frames[0].bpose = dot(H_b0g, H_gc0)
self._frames[1].bpose = dot(H_b1g, H_gc1)
H_c0c1 = dot(Hg.inv(H_gc0), H_gc1)
dsdist = (dot(Hg.adjoint(H_c0c1)[5,:], self._frames[1].twist)
-self._frames[0].twist[5])
self._is_active = (sdist + dsdist*dt < self._proximity)
self._sdist = sdist
self._force[:] = 0.
def add_body(name, mass, com_position, gyration_radius):
#mass matrix at com
mass_g = mass * diag(hstack((gyration_radius**2, (1, 1, 1))))
H_fg = eye(4)
H_fg[0:3, 3] = com_position
H_gf = Hg.inv(H_fg)
#mass matrix at body's frame origin:
mass_o = dot(Hg.adjoint(H_gf).T, dot(mass_g, Hg.adjoint(H_gf)))
bodies.append(Body(name=prefix + name, mass=mass_o))
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
""" Get information on plane/sphere collision.
:param H_g0: pose of the center of the plane relative to the ground
:type H_g0: (4,4)-array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,)-array
:param p_g1: position of center of the sphere relative to the ground
:type p_g1: (3,)-array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the plane and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on plane 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/plane_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
""" Get information on plane/sphere collision.
:param H_g0: pose of the center of the plane relative to the ground
:type H_g0: (4,4)-array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,)-array
:param p_g1: position of center of the sphere relative to the ground
:type p_g1: (3,)-array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the plane and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on plane 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/plane_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
"""
:param H_g0: pose of the plane `H_{g0}`
:type H_g0: (4,4) array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3,3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3,3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
"""
:param H_g0: pose of the plane `H_{g0}`
:type H_g0: (4,4) array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def update(self, dt):
r"""
Compute the predicted relative position error between the socket and
ball centers, `p_{01}(t)` and save it in ``self._pos0``.
.. math::
H_{01}(t)
&= \left(H_{g0}(t)\right)^{-1} \; H_{g1}(t) \\
H_{01}(t)
&=
\begin{bmatrix}
R_{01}(t) & p_{01}(t) \\
0 & 1
\end{bmatrix}
"""
H_01 = dot(Hg.inv(self._frames[0].pose), self._frames[1].pose)
self._pos0 = H_01[0:3, 3]
def update(self, dt):
r"""
Compute the predicted relative position error between the socket and
ball centers, `p_{01}(t)` and save it in ``self._pos0``.
.. math::
H_{01}(t)
&= \left(H_{g0}(t)\right)^{-1} \; H_{g1}(t) \\
H_{01}(t)
&=
\begin{bmatrix}
R_{01}(t) & p_{01}(t) \\
0 & 1
\end{bmatrix}
"""
H_01 = dot(Hg.inv(self._frames[0].pose), self._frames[1].pose)
self._pos0 = H_01[0:3,3]
def update_geometric(self, pose):
"""
- g: ground body
- p: parent body
- c: child body
- r: reference frame of the joint, rigidly fixed to the parent body
- n: new frame of the joint, rigidly fixed to the child body
so H_nc and H_pr are constant.
H_gc = H_gp * H_pc
= H_gp * (H_pr * H_rn * H_nc)
"""
self._pose = pose
H_gp = pose
for j in self.childrenjoints:
H_cn = j._frame1.bpose
H_pr = j._frame0.bpose
H_rn = j.pose
H_pc = dot(H_pr, dot(H_rn, Hg.inv(H_cn)))
child_pose = dot(H_gp, H_pc)
j._frame1.body.update_geometric(child_pose)
def body_com_properties(body, compute_J=True):
""" Compute the Center of Mass properties of a body.
"""
H_body_com = principalframe(body.mass)
H_0_com = dot(body.pose, H_body_com)
P_0_com = H_0_com[0:3, 3]
if compute_J:
H_com_com2 = inv(H_0_com)
H_com_com2[0:3, 3] = 0.
Ad_com2_body = iadjoint(dot(H_body_com, H_com_com2))
J_com2 = dot(Ad_com2_body, body.jacobian)
T_com2_body = body.twist.copy()
T_com2_body[3:6] = 0.
dAd_com2_body = dAdjoint(Ad_com2_body, T_com2_body)
dJ_com2 = dot(Ad_com2_body, body.djacobian) + \
dot(dAd_com2_body, body.jacobian)
return P_0_com, J_com2, dJ_com2
else:
return P_0_com
def body_com_properties(body, compute_J=True):
""" Compute the Center of Mass properties of a body.
"""
H_body_com = principalframe(body.mass)
H_0_com = dot(body.pose, H_body_com)
P_0_com = H_0_com[0:3, 3]
if compute_J:
H_com_com2 = inv(H_0_com)
H_com_com2[0:3, 3] = 0.
Ad_com2_body = iadjoint(dot(H_body_com, H_com_com2))
J_com2 = dot(Ad_com2_body, body.jacobian)
T_com2_body = body.twist.copy()
T_com2_body[3:6] = 0.
dAd_com2_body = dAdjoint(Ad_com2_body, T_com2_body)
dJ_com2 = dot(Ad_com2_body, body.djacobian) + \
dot(dAd_com2_body, body.jacobian)
return P_0_com, J_com2, dJ_com2
else:
return P_0_com
H_bc = transl(1, 1, 1)
else:
H_bc = eye(4)
half_extents = (.5, .5, .5)
mass = 1.
body = Body(name='box_body',
mass=massmatrix.transport(massmatrix.box(half_extents, mass),
H_bc))
subframe = SubFrame(body, H_bc, name="box_com")
if True:
twist_c = array([0., 0., 0., 0., 0., 0.])
else:
twist_c = array([1, 1, 1, 0, 0, 0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, half_extents))
weightc = WeightController()
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
from arboris.visu_osg import Drawer
timeline = arange(0, 1, 5e-3)
simulate(w, timeline, [obs, Drawer(w)])
time = timeline[:-1]
obs.plot_error()
show()
def ipose(self):
"""Inverse of pose
"""
return Hg.inv(self.pose)
H_o_a = Hg.transl(1,2,3) # get a translation homogeneous matrix
H_a_b = Hg.rotzyx(.1,.2,.3) # get a rotation R = Rz * Ry * Rx
# also Hg.rotzy R = Rz * Ry
# Hg.rotzx R = Rz * Rx
# Hg.rotyx R = Ry * Rx
# Hg.rotz R = Rz
# Hg.roty R = Ry
# Hg.rotx R = Rx
H_o_b = np.dot(H_o_a, H_a_b) # H from {o} to {b} is H{o}->{a} * H{a}->{b}
H_b_o = Hg.inv(H_o_b) # obtain inverse, H from {b} to {o}
print("H_o_b * H_b_o\n", np.dot(H_o_b, H_b_o))
isHM =Hg.ishomogeneousmatrix(H_o_b) # check validity of homogeneous matrix
print("is homogeneous matrix?", isHM)
p_b = np.array([.4,.5,.6]) # point p expressed in frame {b}
p_o = Hg.pdot(H_o_b, p_b) # point p expressed in frame {o}
v_b = np.array([.4,.5,.6]) # idem for vector, here affine part is not
v_o = Hg.vdot(H_o_b, v_b) # taken into account
def jacobian(self):
H_01 = dot(Hg.inv(self._frames[0].pose), self._frames[1].pose)
return (dot(Hg.adjoint(H_01)[2:6,:], self._frames[1].jacobian)
-self._frames[0].jacobian[2:6,:])
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
""" Get information on box/sphere collision.
:param H_g0: pose of the center of the box relative to the ground
:type H_g0: (4,4)-array
:param half_extents0: half lengths of the box
:type half_extents0: (3,)-array
:param p_g1: position of the center of the sphere relative to the ground
:type p_g1: (3,) array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the box and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on box 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/box_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> lengths0 = array([1., 2., 3.])
>>> r1 = 0.1
>>> p_g1 = array([0., 3., 1.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
1.9
>>> print(H_gc0)
[[ 0. 1. 0. 0.]
[-0. 0. 1. 1.]
[ 1. -0. 0. 1.]
[ 0. 0. 0. 1.]]
>>> print(H_gc1)
[[ 0. 1. 0. 0. ]
[-0. 0. 1. 2.9]
[ 1. -0. 0. 1. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.55, 0., 0.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.05
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.45]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.45, 0., 0.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.15
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.35]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
if (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
near_face = zeros(6)
near_face[0:3] = half_extents0 - p_01
near_face[3:6] = half_extents0 + p_01
i = argmin(near_face)
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i-3] = -half_extents0[i-3]
normal[i-3] = -1 #TODO check this line is correct
f_g = Hg.pdot(H_g0, f_0)
sdist = -norm(f_g - p_g1)-radius1
else:
# find the point x inside the box that is the nearest to
# the sphere center:
f_0 = zeros(3)
for i in arange(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec/norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(normal)
H_gc1 = H_gc0.copy()
H_gc0[0:3, 3] = f_g
H_gc1[0:3, 3] = p_g1 - radius1*normal
return (sdist, H_gc0, H_gc1)
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
"""
:param H_g0: pose of the box `H_{g0}`
:type H_g0: (4,4) array
:param half_extents0: half lengths of the box
:type half_extents0: (3,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
.. image:: img/box_sphere_collision.png
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> lengths0 = array([1., 2., 3.])
>>> r1 = 0.1
>>> p_g1 = array([0., 3., 1.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
1.9
>>> print(H_gc0)
[[ 0. 1. 0. 0.]
[-0. 0. 1. 1.]
[ 1. -0. 0. 1.]
[ 0. 0. 0. 1.]]
>>> print(H_gc1)
[[ 0. 1. 0. 0. ]
[-0. 0. 1. 2.9]
[ 1. -0. 0. 1. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.55, 0., 0.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.05
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.45]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.45, 0., 0.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.15
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.35]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
if (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
i = argmin(hstack((half_extents0 - p_01, half_extents0 + p_01)))
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i - 3] = -half_extents0[i - 3]
normal[i - 3] = -1 #TODO check this line is correct
f_g = Hg.pdot(H_g0, f_0)
sdist = -norm(f_g - p_g1) - radius1
else:
# find the point x inside the box that is the nearest to
# the sphere center:
f_0 = zeros(3)
for i in range(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec / norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(normal)
H_gc1 = H_gc0.copy()
H_gc0[0:3, 3] = f_g
H_gc1[0:3, 3] = p_g1 - radius1 * normal
return (sdist, H_gc0, H_gc1)
def to_arboris( obj , default=None):
"""
"""
#print "obj:", obj.__class__ #, obj
if isinstance(obj, urdf_parser.Link):
body = arboris.core.Body()
body.name = obj.name
body.mass = to_arboris(obj.inertial)
return body
elif isinstance(obj, urdf_parser.Inertial):
m = obj.mass
H_body_com = to_arboris(obj.origin, np.eye(4))
xx, yy, zz, xy, xz, yz = [obj.matrix["i"+n] for n in ["xx", "yy", "zz", "xy", "xz", "yz"]]
m = max(1e-6, m)
xx = max(1e-6, xx)
yy = max(1e-6, yy)
zz = max(1e-6, zz)
M_com = np.array([[xx, xy, xz, 0., 0., 0.],
[xy, yy, yz, 0., 0., 0.],
[xz, yz, zz, 0., 0., 0.],
[0., 0., 0., m , 0., 0.],
[0., 0., 0., 0., m , 0.],
[0., 0., 0., 0., 0., m ]])
return arboris.massmatrix.transport(M_com, inv(H_body_com) )
elif isinstance(obj, urdf_parser.Pose):
H = arboris.homogeneousmatrix.transl( *obj.position)
H[0:3,0:3] = roll_pitch_yaw_to_rotation_matrix(*obj.rotation)
return H
elif isinstance(obj, urdf_parser.Joint):
joint = {}
joint["parent"] = obj.parent
joint["child"] = obj.child
joint["H_parent_child"] = to_arboris(obj.origin, np.eye(4))
axis = [float(v) for v in obj.axis.split()] if len(obj.axis.split())==3 else obj.axis
if obj.joint_type == urdf_parser.Joint.REVOLUTE:
if (axis in ["x", "X"]) or ( np.allclose(axis, [1,0,0])):
joint["type"] = arboris.joints.RxJoint
elif (axis in ["y", "Y"]) or ( np.allclose(axis, [0,1,0])):
joint["type"] = arboris.joints.RyJoint
elif (axis in ["z", "Z"]) or ( np.allclose(axis, [0,0,1])):
joint["type"] = arboris.joints.RzJoint
else:
joint["H_child_joint"] = arboris.homogeneousmatrix.zaligned(axis)
joint["type"] = arboris.joints.RzJoint
elif obj.joint_type == urdf_parser.Joint.PRISMATIC:
if (axis in ["x", "X"]) or ( np.allclose(axis, [1,0,0])):
joint["type"] = arboris.joints.TxJoint
elif (axis in ["y", "Y"]) or ( np.allclose(axis, [0,1,0])):
joint["type"] = arboris.joints.TyJoint
elif (axis in ["z", "Z"]) or ( np.allclose(axis, [0,0,1])):
joint["type"] = arboris.joints.TzJoint
else:
joint["H_child_joint"] = arboris.homogeneousmatrix.zaligned(axis)
joint["type"] = arboris.joints.TzJoint
elif obj.joint_type == urdf_parser.Joint.FIXED:
joint["type"] = arboris.joints.FixedJoint
elif obj.joint_type == urdf_parser.Joint.FLOATING:
print "WARNING: urdf containt free joint; this joint is deprecated."
joint["type"] = arboris.joints.FreeJoint
else:
raise KeyError, "Joint type: '"+obj.joint_type+"' is unknown or not not implemented yet."
return joint
elif isinstance(obj, urdf_parser.Visual):
visual = to_arboris(obj.geometry)
visual["transform"] = to_arboris(obj.origin, np.eye(4))
#material = to_arboris(obj.material)
return visual
elif isinstance(obj, urdf_parser.Collision):
collision = to_arboris(obj.geometry)
collision["transform"] = to_arboris(obj.origin, np.eye(4))
return collision
elif isinstance(obj, urdf_parser.Mesh):
mesh_path = obj.filename
scale = 1 if obj.scale is None else [float(v) for v in obj.scale.split()]
return {"shape": "mesh", "mesh_from_urdf": mesh_path, "scale": scale}
elif isinstance(obj, urdf_parser.Box):
mesh_path = arboris_simple_shapes_path + "#simple_box_node"
scale = obj.dims
return {"shape": "box", "mesh": mesh_path, "scale": scale}
elif isinstance(obj, urdf_parser.Sphere):
mesh_path = arboris_simple_shapes_path + "#simple_sphere_node"
scale = obj.radius
return {"shape": "sphere", "mesh": mesh_path, "scale": scale}
elif isinstance(obj, urdf_parser.Cylinder):
mesh_path = arboris_simple_shapes_path + "#simple_cylinder_node"
scale = [obj.radius, obj.radius, obj.length]
return {"shape": "cylinder", "mesh": mesh_path, "scale": scale}
elif isinstance(obj, None.__class__):
if default is not None:
return default
else:
print "WARNING: Cannot load object", obj.__class__, obj
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
"""
:param H_g0: pose of the box `H_{g0}`
:type H_g0: (4,4) array
:param half_extents0: half lengths of the box
:type half_extents0: (3,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
.. image:: img/box_sphere_collision.png
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> lengths0 = array([1., 2., 3.])
>>> r1 = 0.1
>>> p_g1 = array([0., 3., 1.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
1.9
>>> print(H_gc0)
[[ 0. 1. 0. 0.]
[-0. 0. 1. 1.]
[ 1. -0. 0. 1.]
[ 0. 0. 0. 1.]]
>>> print(H_gc1)
[[ 0. 1. 0. 0. ]
[-0. 0. 1. 2.9]
[ 1. -0. 0. 1. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.55, 0., 0.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.05
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.45]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.45, 0., 0.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.15
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.35]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
if (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
i = argmin(hstack((half_extents0 - p_01, half_extents0 + p_01)))
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i-3] = -half_extents0[i-3]
normal[i-3] = -1 #TODO check this line is correct
f_g = Hg.pdot(H_g0, f_0)
sdist = -norm(f_g - p_g1)-radius1
else:
# find the point x inside the box that is the nearest to
# the sphere center:
f_0 = zeros(3)
for i in range(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec/norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(normal)
H_gc1 = H_gc0.copy()
H_gc0[0:3, 3] = f_g
H_gc1[0:3, 3] = p_g1 - radius1*normal
return (sdist, H_gc0, H_gc1)
def add(w, is_fixed=False, create_shapes=True, create_contacts=True):
"""
construction of the icub robot for arboris-python:
Kinematics data are from: http://eris.liralab.it/wiki/ICubForwardKinematics
Inertia comes from the Icub.cpp used in the iCub_SIM program
Some data are not well explained, or are badly defined
"""
bodies_data = get_bodies_data()
bodies_shapes_data = get_bodies_shapes_data()
joints_data = get_joints_data()
shapes_data = get_contact_data()
## bodies creation
bodies = {}
for name, data in bodies_data.items():
bodies[name] = Body(name=name)
mass = zeros((6, 6))
for dims, m, H in data: # get dims, mass and transformation from data
sf = SubFrame(bodies[name], H)
if len(dims) == 3: # check the type of shape: len =3: box
M = box(dims, m)
elif len(dims) == 2: # len =2:cylinder,
M = cylinder(dims[0], dims[1], m)
elif len(dims) == 1: # len =1:sphere,
M = sphere(dims[0], m)
else:
raise ValueError
mass += transport(M, inv(H)) # add the mass of the shape to
bodies[name].mass = mass # the total mass
## check if iCub has its waist fixed on the structure (the ground)
if is_fixed:
bodies['waist'] = w.ground
else:
w.add_link(w.ground, FreeJoint(name='root'), bodies['waist'])
## joints creation
for name, data in joints_data.items():
parent, Hp_l, child = data
w.add_link(SubFrame(bodies[parent], Hp_l),
RzJoint(name=name),
bodies[child])
if create_shapes is True:
## body shapes creations
for name, data in bodies_shapes_data.items():
for dims, H in data: # get dims, mass and transformation from data
sf = SubFrame(bodies[name], H)
if len(dims) == 3: # check the type of shape: len =3: box
sh = Box(sf, dims, name+".body_shape")
elif len(dims) == 2: # len =2:cylinder,
sh = Cylinder(sf, dims[0], dims[1], name+".body_shape")
elif len(dims) == 1: # len =1:sphere,
sh = Sphere(sf, dims[0], name+".body_shape")
else:
raise ValueError
w.register(sh)
if create_contacts is True:
## contact shapes creation
for name, data in shapes_data.items():
parent, dims, Hpf = data
sf = SubFrame(bodies[parent], Hpf, name=name)
if len(dims) == 3: # check the type of shape: len =3: box
sh = Box(sf, dims, name=name)
elif len(dims) == 2: # len =2:cylinder,
sh = Cylinder(sf, dims[0], dims[1], name=name)
elif len(dims) == 1: # len =1:sphere,
sh = Sphere(sf, dims[0], name=name)
else:
sh = Point(sf, name=name)
w.register(sh)
w.init()
def update_dynamic(self, pose, jac, djac, twist):
r"""Sets the body ``pose, jac, djac, twist`` and computes its children ones.
This method (1) sets the body dynamical model (pose, jacobian,
hessian and twist) to the values given as argument, (2) computes
the dynamical model of the children bodies and (3) call the
equivalent method on them.
As a result, the dynamical model of all the bodies is computed
recursively.
:param pose: the body pose relative to the ground: `H_{gb}`
:type pose: 4x4 ndarray
:param jac: the body jacobian relative to the world (in body frame):
`\J[b]_{b/g}`
:type jac: 6x(ndof) ndarray
:param djac: the derivative of the body jacobian: `\dJ[b]_{b/g}`
:param twist: the body twist: `\twist[b]_{b/g}`
:type twist: 6 ndarray
**Algorithm:**
Let's define the following notations:
- `g`: the ground body,
- `p`: the parent body (which is the present :class:`arboris.Body`
instance)
- `c`: a child body,
- `j`: the joint between the bodies `p` and `c`,
- `r`: reference frame of the joint `j`, rigidly fixed to the parent
body
- `n`: new frame of the joint `j`, rigidly fixed to the child body
.. image:: img/body_model.png
One can notice that `H_{nc}` and `H_{pr}` are constant.
The child body pose can be computed as
.. math::
H_{gc} &= H_{gp} \; H_{pc} \\
&= H_{gp} \; (H_{pr} \; H_{rn} \; H_{nc})
where `H_{rn}` depends on the joint generalized configuration and is
given by its :attr:`~arboris.core.Joint.pose` attribute.
The chil body twist is given as
.. math::
\twist[c]_{c/g} &= \Ad[c]_p \; \twist[p]_{p/g} + \twist[c]_{c/p} \\
&= \Ad[c]_p \; \twist[p]_{p/g} + \Ad[c]_n \; \twist[n]_{n/r} \\
&= \Ad[c]_p \; \J[p]_{p/g} \; \GVel
+ \Ad[c]_n \; \J[n]_{n/r} \; \GVel_j \\
&= \J[c]_{c/g} \; \GVel
where `\twist[n]_{n/r}` isgiven by the joint
:attr:`~arboris.core.Joint.twist` attribute.
\GVel_j is the generalized velocity of the joint `j` and is
related to the world generalized velocity by trivial projection
.. math::
\GVel_j &=
\begin{bmatrix}
0 & \cdots &0 & I & 0 & \cdots & 0
\end{bmatrix} \; \GVel
therefore, the child body jacobian is
.. math::
\J[c]_{c/g} &= \Ad[c]_p \; \J[p]_{p/g} +
\begin{bmatrix}
0 & \cdots & 0 & \Ad[c]_n \; \J[n]_{n/r} & 0 & \cdots & 0
\end{bmatrix} \\
where `\J[n]_{n/r}` is given by the joint
:attr:`~arboris.core.Joint.jacobian` attribute. Derivating the previous
expression leads to the child body acceleration:
.. math::
\dtwist[c]_{c/g} &= \dAd[c]_p \; \J[p]_{p/g} \; \GVel
+ \Ad[c]_p \; \dJ[p]_{p/g} \; \GVel
+ \Ad[c]_p \; \J[p]_g \; \dGVel
+ \Ad[c]_n \; \dJ[n]_{n/r} \; \GVel_j
+ \Ad[c]_n \; \J[n]_{m/r} \dGVel_j \\
&= \J[c]_{c/g} \; \dGVel + \dJ[c]_{c/g} \; \GVel
the expression of the child body hessian is then obtained by
identification:
.. math::
\dJ[c]_{c/g} \; \GVel
&= \dAd[c]_p \; \J[p]_{p/g} \; \GVel
+ \Ad[c]_p \; \dJ[p]_{p/g} \; \GVel
+ \Ad[c]_n \; \dJ[n]_{n/r} \; \GVel_j \\
\dJ[c]_{c/g}
&= \dAd[c]_p \; \J[p]_{p/g} + \Ad[c]_p \; \dJ[p]_{p/g} +
\begin{bmatrix}
0 & \cdots & 0 & (\Ad[c]_n \; \dJ[n]_{n/r}) & 0 & \cdots & 0
\end{bmatrix}
with
.. math::
\dAd[c]_p &= \Ad[c]_n \; \dAd[n]_r \; \Ad[r]_p
and where `\dAd[n]_r` and `\dJ[n]_{n/r}` are respectively given by
the joint :attr:`~arboris.core.Joint.idadjoint` and
:attr:`~arboris.core.Joint.djacobian` attributes.
T_ab: velocity of {a} relative to {b} expressed in {a} (body twist)
"""
self._pose = pose
self._jacobian = jac
self._djacobian = djac
self._twist = twist
wx = array(
[[ 0, -self.twist[2], self.twist[1]],
[ self.twist[2], 0, -self.twist[0]],
[-self.twist[1], self.twist[0], 0]])
if self.mass[3, 3] <= 1e-10: #TODO: avoid hardcoded value
rx = zeros((3, 3))
else:
rx = self.mass[0:3, 3:6]/self.mass[3,3] #TODO: better solution?
self._nleffects = zeros((6, 6))
self._nleffects[0:3, 0:3] = wx
self._nleffects[3:6, 3:6] = wx
self._nleffects[0:3, 3:6] = dot(rx, wx) - dot(wx, rx)
self._nleffects = dot(self.nleffects, self.mass)
H_gp = pose
J_pg = jac
dJ_pg = djac
T_pg = twist
for j in self.childrenjoints:
H_cn = j._frame1.bpose
H_pr = j._frame0.bpose
H_rn = j.pose
H_pc = dot(H_pr, dot(H_rn, Hg.inv(H_cn)))
child_pose = dot(H_gp, H_pc)
Ad_cp = Hg.iadjoint(H_pc)
Ad_cn = Hg.adjoint(H_cn)
Ad_rp = Hg.adjoint(Hg.inv(H_pr))
dAd_nr = j.idadjoint
dAd_cp = dot(Ad_cn, dot(dAd_nr, Ad_rp))
T_nr = j.twist
J_nr = j.jacobian
dJ_nr = j.djacobian
child_twist = dot(Ad_cp, T_pg) + dot(Ad_cn, T_nr)
child_jac = dot(Ad_cp, J_pg)
child_jac[:,j.dof] += dot(Ad_cn, J_nr)
child_djac = dot(dAd_cp, J_pg) + dot(Ad_cp, dJ_pg)
child_djac[:, j.dof] += dot(Ad_cn, dJ_nr)
j._frame1.body.update_dynamic(child_pose, child_jac, child_djac,
child_twist)
def jacobian(self):
H_01 = dot(Hg.inv(self._frames[0].pose), self._frames[1].pose)
return (dot(Hg.adjoint(H_01)[2:6, :], self._frames[1].jacobian) -
self._frames[0].jacobian[2:6, :])
def _human36(world, height=1.741, mass=73, name=''):
"""
TODO: HuMAnS' doc about inertia is erroneous (the real math is in the IOMatrix proc in DynamicData.maple)
"""
assert isinstance(world, World)
w = world
L = anatomical_lengths(height)
bodies = {}
humansbodyid_to_humansbodyname_map = {}
for b in _humans_bodies(height, mass):
#mass matrix at com
mass_g = b['Mass'] * diag(
hstack((b['GyrationRadius']**2, (1,1,1))))
H_fg = eye(4)
H_fg[0:3,3] = b['CenterOfMass']
H_gf = Hg.inv(H_fg)
#mass matrix at body's frame origin:
mass_o = dot(adjoint(H_gf).T, dot(mass_g, adjoint(H_gf)))
bodies[b['HumansName']] = Body(
name=b['HumansName'],
mass=mass_o)
humansbodyid_to_humansbodyname_map[b['HumansId']] = b['HumansName']
rf = SubFrame(w.ground,
Hg.transl(0, L['yfootL']+L['ytibiaL']+L['yfemurL'], 0))
w.add_link(rf, FreeJoint(), bodies['LPT'])
rf = SubFrame(bodies['LPT'], Hg.transl(0, 0, L['zhip']/2.))
j = RzRyRxJoint()
w.add_link(rf, j, bodies['ThighR'])
rf = SubFrame(bodies['ThighR'], Hg.transl(0, -L['yfemurR'], 0))
w.add_link(rf, RzJoint(), bodies['ShankR'])
rf = SubFrame(bodies['ShankR'], Hg.transl(0, -L['ytibiaR'], 0))
w.add_link(rf, RzRxJoint(), bodies['FootR'])
rf = SubFrame(bodies['LPT'], Hg.transl(0, 0, -L['zhip']/2.))
w.add_link(rf, RzRyRxJoint(), bodies['ThighL'])
rf = SubFrame(bodies['ThighL'], Hg.transl(0, -L['yfemurL'], 0))
w.add_link(rf, RzJoint(), bodies['ShankL'])
rf = SubFrame(bodies['ShankL'], Hg.transl(0, -L['ytibiaL'], 0))
w.add_link(rf, RzRxJoint(), bodies['FootL'])
rf = SubFrame(bodies['LPT'], Hg.transl(-L['xvT10'], L['yvT10'], 0))
w.add_link(rf, RzRyRxJoint(), bodies['UPT'])
rf = SubFrame(bodies['UPT'],
Hg.transl(L['xsternoclavR'], L['ysternoclavR'], L['zsternoclavR']))
j = RyRxJoint()
w.add_link(rf, j, bodies['ScapulaR'])
rf = SubFrame(bodies['ScapulaR'],
Hg.transl(-L['xshoulderR'], L['yshoulderR'], L['zshoulderR']))
w.add_link(rf, RzRyRxJoint(), bodies['ArmR'])
rf = SubFrame(bodies['ArmR'], Hg.transl(0, -L['yhumerusR'], 0))
w.add_link(rf, RzRyJoint(), bodies['ForearmR'])
rf = SubFrame(bodies['ForearmR'], Hg.transl(0, -L['yforearmR'], 0))
w.add_link(rf, RzRxJoint(), bodies['HandR'])
rf = SubFrame(bodies['UPT'], Hg.transl(
L['xsternoclavL'], L['ysternoclavL'], -L['zsternoclavL']))
w.add_link(rf, RyRxJoint(), bodies['ScapulaL'])
rf = SubFrame(bodies['ScapulaL'],
Hg.transl(-L['xshoulderL'], L['yshoulderL'], -L['zshoulderL']))
w.add_link(rf, RzRyRxJoint(), bodies['ArmL'])
rf = SubFrame(bodies['ArmL'], Hg.transl(0, -L['yhumerusL'], 0))
w.add_link(rf, RzRyJoint(), bodies['ForearmL'])
rf = SubFrame(bodies['ForearmL'], Hg.transl(0, -L['yforearmL'], 0))
w.add_link(rf, RzRxJoint(), bodies['HandL'])
rf = SubFrame(bodies['UPT'], Hg.transl(L['xvT10'], L['yvC7'], 0))
w.add_link(rf, RzRyRxJoint(), bodies['Head'])
# add tags
tags = {}
for t in _humans_tags(height):
bodyname = humansbodyid_to_humansbodyname_map[t['HumansBodyId']]
tag = SubFrame(
bodies[bodyname],
Hg.transl(t['Position'][0], t['Position'][1], t['Position'][2]),
t['HumansName'])
tags[t['HumansName']] = tag
w.register(tag)
# Add point shapes to the feet
for k in ('Right foot toe tip', 'Right foot heel',
'Right foot phalange 5', 'Right foot Phalange 1',
'Left foot toe tip','Left foot heel',
'Left foot phalange 5','Left foot phalange 1'):
shape = Point(tags[k], name=k)
w.register(shape)
w.init()
return (bodies, tags)
def add(w, is_fixed=False, create_shapes=True, create_contacts=True):
"""
construction of the icub robot for arboris-python:
Kinematics data are from: http://eris.liralab.it/wiki/ICubForwardKinematics
Inertia comes from the Icub.cpp used in the iCub_SIM program
Some data are not well explained, or are badly defined
"""
bodies_data = get_bodies_data()
bodies_shapes_data = get_bodies_shapes_data()
joints_data = get_joints_data()
shapes_data = get_contact_data()
## bodies creation
bodies = {}
for name, data in bodies_data.items():
bodies[name] = Body(name=name)
mass = zeros((6, 6))
for dims, m, H in data: # get dims, mass and transformation from data
sf = SubFrame(bodies[name], H)
if len(dims) == 3: # check the type of shape: len =3: box
M = box(dims, m)
elif len(dims) == 2: # len =2:cylinder,
M = cylinder(dims[0], dims[1], m)
elif len(dims) == 1: # len =1:sphere,
M = sphere(dims[0], m)
else:
raise ValueError
mass += transport(M, inv(H)) # add the mass of the shape to
bodies[name].mass = mass # the total mass
## check if iCub has its waist fixed on the structure (the ground)
if is_fixed:
bodies['waist'] = w.ground
else:
w.add_link(w.ground, FreeJoint(name='root'), bodies['waist'])
## joints creation
for name, data in joints_data.items():
parent, Hp_l, child = data
w.add_link(SubFrame(bodies[parent], Hp_l), RzJoint(name=name),
bodies[child])
if create_shapes is True:
## body shapes creations
for name, data in bodies_shapes_data.items():
for dims, H in data: # get dims, mass and transformation from data
sf = SubFrame(bodies[name], H)
if len(dims) == 3: # check the type of shape: len =3: box
sh = Box(sf, dims, name + ".body_shape")
elif len(dims) == 2: # len =2:cylinder,
sh = Cylinder(sf, dims[0], dims[1], name + ".body_shape")
elif len(dims) == 1: # len =1:sphere,
sh = Sphere(sf, dims[0], name + ".body_shape")
else:
raise ValueError
w.register(sh)
if create_contacts is True:
## contact shapes creation
for name, data in shapes_data.items():
parent, dims, Hpf = data
sf = SubFrame(bodies[parent], Hpf, name=name)
if len(dims) == 3: # check the type of shape: len =3: box
sh = Box(sf, dims, name=name)
elif len(dims) == 2: # len =2:cylinder,
sh = Cylinder(sf, dims[0], dims[1], name=name)
elif len(dims) == 1: # len =1:sphere,
sh = Sphere(sf, dims[0], name=name)
else:
sh = Point(sf, name=name)
w.register(sh)
w.init()
def ipose(self):
"""Inverse of pose
"""
return Hg.inv(self.pose)
def update_dynamic(self, pose, jac, djac, twist):
r""" Compute the body ``pose, jac, djac, twist`` and its children ones.
This method (1) sets the body dynamical model (pose, jacobian,
hessian and twist) to the values given as argument, (2) computes
the dynamical model of the children bodies and (3) call the
equivalent method on them.
As a result, the dynamical model of all the bodies is computed
recursively.
:param pose: the body pose relative to the ground: `H_{gb}`
:type pose: 4x4 ndarray
:param jac: the body jacobian relative to the world (in body frame):
`\J[b]_{b/g}`
:type jac: 6x(ndof) ndarray
:param djac: the derivative of the body jacobian: `\dJ[b]_{b/g}`
:param twist: the body twist: `\twist[b]_{b/g}`
:type twist: 6 ndarray
**Algorithm:**
Let's define the following notations:
- `g`: the ground body,
- `p`: the parent body (which is the present :class:`arboris.Body`
instance)
- `c`: a child body,
- `j`: the joint between the bodies `p` and `c`,
- `r`: reference frame of the joint `j`, rigidly fixed to the parent
body
- `n`: new frame of the joint `j`, rigidly fixed to the child body
.. image:: img/body_model.svg
:width: 300px
One can notice that `H_{nc}` and `H_{pr}` are constant.
The child body pose can be computed as
.. math::
H_{gc} &= H_{gp} \; H_{pc} \\
&= H_{gp} \; (H_{pr} \; H_{rn} \; H_{nc})
where `H_{rn}` depends on the joint generalized configuration and is
given by its :attr:`~arboris.core.Joint.pose` attribute.
The chil body twist is given as
.. math::
\twist[c]_{c/g} &= \Ad_{cp} \; \twist[p]_{p/g} + \twist[c]_{c/p} \\
&= \Ad_{cp} \; \twist[p]_{p/g} + \Ad_{cn} \; \twist[n]_{n/r} \\
&= \Ad_{cp} \; \J[p]_{p/g} \; \GVel
+ \Ad_{cn} \; \J[n]_{n/r} \; \GVel_j \\
&= \J[c]_{c/g} \; \GVel
where `\twist[n]_{n/r}` is given by the joint
:attr:`~arboris.core.Joint.twist` attribute.
`\GVel_j` is the generalized velocity of the joint `j` and is
related to the world generalized velocity by trivial projection
.. math::
\GVel_j &=
\begin{bmatrix}
0 & \cdots &0 & I & 0 & \cdots & 0
\end{bmatrix} \; \GVel
therefore, the child body jacobian is
.. math::
\J[c]_{c/g} &= \Ad_{cp} \; \J[p]_{p/g} +
\begin{bmatrix}
0 & \cdots & 0 & \Ad_{cn} \; \J[n]_{n/r} & 0 & \cdots & 0
\end{bmatrix} \\
where `\J[n]_{n/r}` is given by the joint
:attr:`~arboris.core.Joint.jacobian` attribute. Derivating the previous
expression leads to the child body acceleration:
.. math::
\dtwist[c]_{c/g} &= \dAd_{cp} \; \J[p]_{p/g} \; \GVel
+ \Ad_{cp} \; \dJ[p]_{p/g} \; \GVel
+ \Ad_{cp} \; \J[p]_g \; \dGVel
+ \Ad_{cn} \; \dJ[n]_{n/r} \; \GVel_j
+ \Ad_{cn} \; \J[n]_{m/r} \dGVel_j \\
&= \J[c]_{c/g} \; \dGVel + \dJ[c]_{c/g} \; \GVel
the expression of the child body hessian is then obtained by
identification:
.. math::
\dJ[c]_{c/g} \; \GVel
&= \dAd_{cp} \; \J[p]_{p/g} \; \GVel
+ \Ad_{cp} \; \dJ[p]_{p/g} \; \GVel
+ \Ad_{cn} \; \dJ[n]_{n/r} \; \GVel_j \\
\dJ[c]_{c/g}
&= \dAd_{cp} \; \J[p]_{p/g} + \Ad_{cp} \; \dJ[p]_{p/g} +
\begin{bmatrix}
0 & \cdots & 0 & (\Ad_{cn} \; \dJ[n]_{n/r}) & 0 & \cdots & 0
\end{bmatrix}
with
.. math::
\dAd_{cp} &= \Ad_{cn} \; \dAd_{nr} \; \Ad_{rp}
and where `\dAd_{nr}` and `\dJ[n]_{n/r}` are respectively given by
the joint :attr:`~arboris.core.Joint.idadjoint` and
:attr:`~arboris.core.Joint.djacobian` attributes.
T_ab: velocity of {a} relative to {b} expressed in {a} (body twist)
"""
self._pose = pose
self._jacobian = jac
self._djacobian = djac
self._twist = twist
wx = array([[0, -self.twist[2], self.twist[1]],
[self.twist[2], 0, -self.twist[0]],
[-self.twist[1], self.twist[0], 0]])
if self.mass[5, 5] <= 1e-10: #TODO: avoid hardcoded value
rx = zeros((3, 3))
else:
rx = self.mass[0:3, 3:6] / self.mass[5, 5]
self._nleffects = zeros((6, 6))
self._nleffects[0:3, 0:3] = wx
self._nleffects[3:6, 3:6] = wx
self._nleffects[0:3, 3:6] = dot(rx, wx) - dot(wx, rx)
self._nleffects = dot(self.nleffects, self.mass)
H_gp = pose
J_pg = jac
dJ_pg = djac
T_pg = twist
for j in self.childrenjoints:
H_cn = j.frame1.bpose
H_pr = j.frame0.bpose
H_rn = j.pose
H_pc = dot(H_pr, dot(H_rn, Hg.inv(H_cn)))
child_pose = dot(H_gp, H_pc)
Ad_cp = Hg.iadjoint(H_pc)
Ad_cn = Hg.adjoint(H_cn)
Ad_rp = Hg.adjoint(Hg.inv(H_pr))
dAd_nr = j.idadjoint
dAd_cp = dot(Ad_cn, dot(dAd_nr, Ad_rp))
T_nr = j.twist
J_nr = j.jacobian
dJ_nr = j.djacobian
child_twist = dot(Ad_cp, T_pg) + dot(Ad_cn, T_nr)
child_jac = dot(Ad_cp, J_pg)
child_jac[:, j.dof] += dot(Ad_cn, J_nr)
child_djac = dot(dAd_cp, J_pg) + dot(Ad_cp, dJ_pg)
child_djac[:, j.dof] += dot(Ad_cn, dJ_nr)
j.frame1.body.update_dynamic(child_pose, child_jac, child_djac,
child_twist)
H_bc = transl(1,1,1)
else:
H_bc = eye(4)
lengths = (1.,1.,1.)
mass = 1.
body = Body(
name='box_body',
mass=massmatrix.transport(massmatrix.box(lengths, mass), H_bc))
subframe = SubFrame(body, H_bc, name="box_com")
if True:
twist_c = array([0.,0.,0.,0.,0.,0.])
else:
twist_c = array([1,1,1,0,0,0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, lengths))
weightc = WeightController(w)
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
w.observers.append(obs)
from arboris.visu_osg import Drawer
w.observers.append(Drawer(w))
timeline = arange(0,1,5e-3)
simulate(w, timeline)
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
""" Get information on box/sphere collision.
:param H_g0: pose of the center of the box relative to the ground
:type H_g0: (4,4)-array
:param half_extents0: half lengths of the box
:type half_extents0: (3,)-array
:param p_g1: position of the center of the sphere relative to the ground
:type p_g1: (3,) array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the box and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on box 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/box_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> lengths0 = array([1., 2., 3.])
>>> r1 = 0.1
>>> p_g1 = array([0., 3., 1.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
1.9
>>> print(H_gc0)
[[ 0. 1. 0. 0.]
[-0. 0. 1. 1.]
[ 1. -0. 0. 1.]
[ 0. 0. 0. 1.]]
>>> print(H_gc1)
[[ 0. 1. 0. 0. ]
[-0. 0. 1. 2.9]
[ 1. -0. 0. 1. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.55, 0., 0.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.05
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.45]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.45, 0., 0.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.15
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.35]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
if (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
near_face = zeros(6)
near_face[0:3] = half_extents0 - p_01
near_face[3:6] = half_extents0 + p_01
i = argmin(near_face)
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i - 3] = -half_extents0[i - 3]
normal[i - 3] = -1 #TODO check this line is correct
f_g = Hg.pdot(H_g0, f_0)
sdist = -norm(f_g - p_g1) - radius1
else:
# find the point x inside the box that is the nearest to
# the sphere center:
f_0 = zeros(3)
for i in arange(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec / norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(normal)
H_gc1 = H_gc0.copy()
H_gc0[0:3, 3] = f_g
H_gc1[0:3, 3] = p_g1 - radius1 * normal
return (sdist, H_gc0, H_gc1)
