def update(self): 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) #self._frames[0].bpose = dot(self._shapes[0].frame.bpose, H_s0c0) #self._frames[1].bpose = dot(self._shapes[1].frame.bpose, H_s1c1) 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) #TODO: use a set method? self._frames[1]._bpose = dot(H_b1g, H_gc1) self._is_active = (sdist < self._proximity) self._sdist = sdist
def update(self): 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 _box_sphere_collision(H_g0, lengths0, p_g1, radius1): """ :param H_g0: pose of the box `H_{g1}` :type H_g0: (4,4) array :param lengths0: lengths of the box :type lengths0: (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, 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, 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, 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 (-lengths0[0]/2. <= p_01[0] and p_01[0] <= lengths0[0]/2.) and\ (-lengths0[1]/2. <= p_01[1] and p_01[1] <= lengths0[1]/2.) and\ (-lengths0[2]/2. <= p_01[2] and p_01[2] <= lengths0[2]/2.): # p_01 is inside the box, we need to find the nearest face i = argmin(hstack((lengths0 - p_01, lengths0 + p_01))) f_0 = p_01.copy() normal = zeros(3) if i < 3: f_0[i] = lengths0[i]/2. normal[i] = 1 else: f_0[i-3] = -lengths0[i-3]/2. 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(lengths0[i]/2., p_01[i]), -lengths0[i]/2.) f_g = Hg.pdot(H_g0, f_0) vec = p_g1 - f_g normal = vec/norm(vec) sdist = norm(vec) - radius1 H_gc0 = _normal_to_frame(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 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,:])