void SpringForce::addGradEToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, VectorXs& gradE )
{
assert( x.size() == v.size() );
assert( x.size() == gradE.size() );
assert( x.size()%2 == 0 );
assert( m_endpoints.first >= 0 ); assert( m_endpoints.first < x.size()/2 );
assert( m_endpoints.second >= 0 ); assert( m_endpoints.second < x.size()/2 );
//GET PARTICLES
int size = 2;
int xistart = m_endpoints.first*2;
int xjstart = m_endpoints.second*2;
VectorXs xi = x.segment(xistart, size);
VectorXs xj = x.segment(xjstart, size);
//SPRING FORCE
VectorXs nhat = xi-xj;
double lxixj = nhat.norm();
nhat /= lxixj;
VectorXs dxiU = m_k*(lxixj-m_l0)*nhat;
VectorXs dxjU = -dxiU;
//SPRING DAMPING FORCE
VectorXs vi = v.segment(xistart, size);
VectorXs vj = v.segment(xjstart, size);
double lvivj = (vi-vj).dot(nhat);
dxiU += m_b * nhat * lvivj; //F-
dxjU += -m_b * nhat * lvivj; //F+
//CHANGE SYSTEM
gradE[xistart+0] += dxiU[0];
gradE[xistart+1] += dxiU[1];
gradE[xjstart+0] += dxjU[0];
gradE[xjstart+1] += dxjU[1];
}
DrakeSystem::VectorXs CascadeSystem::getX1(const VectorXs &x) const {
DrakeSystem::VectorXs x1(sys1->state_frame->getDim());
x1 << x.head(sys1->continuous_state_frame->getDim()),
x.segment(sys1->state_frame->getDim(),sys1->discrete_state_frame->getDim());
return x1;
}
DrakeSystem::VectorXs CascadeSystem::getX2(const VectorXs &x) const {
DrakeSystem::VectorXs x2(sys2->state_frame->getDim());
x2 << x.segment(sys1->continuous_state_frame->getDim(),sys2->continuous_state_frame->getDim()),
x.segment(sys1->state_frame->getDim()+sys2->continuous_state_frame->getDim(),sys2->discrete_state_frame->getDim());
return x2;
}