void vpSystem::IntegrateDynamicsEuler(scalar time_step)
{
ForwardDynamics();
for ( int i = 0; i < m_pJoint.size(); i++ )
{
m_pJoint[i]->IntegrateDisplacement(time_step);
m_pJoint[i]->IntegrateVelocity(time_step);
}
if ( !m_pRoot->m_bIsGround )
{
m_pRoot->m_sFrame *= Exp(time_step * m_pRoot->m_sV);
m_pRoot->m_sV += time_step * m_pRoot->m_sDV;
}
Reparameterize();
}
/*
* FitCubic :
* Fit a Bezier curve to a (sub)set of digitized points
*/
static void FitCubic(Point2 *d, int first, int last, Vector2 tHat1, Vector2 tHat2,
double error, BezierContour &bezContour)
{
BezierCurve bezCurve; /*Control points of fitted Bezier curve*/
double *u; /* Parameter values for point */
double *uPrime; /* Improved parameter values */
double maxError; /* Maximum fitting error */
int splitPoint; /* Point to split point set at */
int nPts; /* Number of points in subset */
double iterationError; /*Error below which you try iterating */
int maxIterations = 4; /* Max times to try iterating */
Vector2 tHatCenter; /* Unit tangent vector at splitPoint */
int i;
iterationError = error * error;
nPts = last - first + 1;
/* Use heuristic if region only has two points in it */
if (nPts == 2) {
double dist = V2DistanceBetween2Points(&d[last], &d[first]) / 3.0;
bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
bezCurve[0] = d[first];
bezCurve[3] = d[last];
V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
DrawBezierCurve(3, bezCurve, bezContour);
free((void *)bezCurve);
return;
}
/* Parameterize points, and attempt to fit curve */
u = ChordLengthParameterize(d, first, last);
bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
/* Find max deviation of points to fitted curve */
maxError = ComputeMaxError(d, first, last, bezCurve, u, &splitPoint);
if (maxError < error) {
DrawBezierCurve(3, bezCurve, bezContour);
free((void *)u);
free((void *)bezCurve);
return;
}
/* If error not too large, try some reparameterization */
/* and iteration */
if (maxError < iterationError) {
for (i = 0; i < maxIterations; i++) {
uPrime = Reparameterize(d, first, last, u, bezCurve);
free((void *)bezCurve);
bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
maxError = ComputeMaxError(d, first, last,
bezCurve, uPrime, &splitPoint);
if (maxError < error) {
DrawBezierCurve(3, bezCurve, bezContour);
free((void *)u);
free((void *)bezCurve);
free((void *)uPrime);
return;
}
free((void *)u);
u = uPrime;
}
}
/* Fitting failed -- split at max error point and fit recursively */
free((void *)u);
free((void *)bezCurve);
tHatCenter = ComputeCenterTangent(d, splitPoint);
FitCubic(d, first, splitPoint, tHat1, tHatCenter, error, bezContour);
V2Negate(&tHatCenter);
FitCubic(d, splitPoint, last, tHatCenter, tHat2, error, bezContour);
}
QPointF *FitCubic(const QList<QPointF> &points, int first, int last, FitVector tHat1, FitVector tHat2, float error, int &width)
{
qreal *u;
qreal *uPrime;
qreal maxError;
int splitPoint;
int nPts;
qreal iterationError;
int maxIterations = 4;
FitVector tHatCenter;
QPointF *curve;
int i;
width = 0;
iterationError = error * error;
nPts = last - first + 1;
if (nPts == 2) {
qreal dist = distance(points.at(last), points.at(first)) / 3.0;
curve = new QPointF[4];
curve[0] = points.at(first);
curve[3] = points.at(last);
tHat1.scale(dist);
tHat2.scale(dist);
curve[1] = tHat1 + curve[0];
curve[2] = tHat2 + curve[3];
width = 4;
return curve;
}
/* Parameterize points, and attempt to fit curve */
u = ChordLengthParameterize(points, first, last);
curve = GenerateBezier(points, first, last, u, tHat1, tHat2);
/* Find max deviation of points to fitted curve */
maxError = ComputeMaxError(points, first, last, curve, u, &splitPoint);
if (maxError < error) {
delete[] u;
width = 4;
return curve;
}
/* If error not too large, try some reparameterization */
/* and iteration */
if (maxError < iterationError) {
for (i = 0; i < maxIterations; ++i) {
uPrime = Reparameterize(points, first, last, u, curve);
delete[] curve;
curve = GenerateBezier(points, first, last, uPrime, tHat1, tHat2);
maxError = ComputeMaxError(points, first, last,
curve, uPrime, &splitPoint);
if (maxError < error) {
delete[] u;
delete[] uPrime;
width = 4;
return curve;
}
delete[] u;
u = uPrime;
}
}
/* Fitting failed -- split at max error point and fit recursively */
delete[] u;
delete[] curve;
tHatCenter = ComputeCenterTangent(points, splitPoint);
int w1, w2;
QPointF *cu1 = NULL, *cu2 = NULL;
cu1 = FitCubic(points, first, splitPoint, tHat1, tHatCenter, error, w1);
tHatCenter.negate();
cu2 = FitCubic(points, splitPoint, last, tHatCenter, tHat2, error, w2);
QPointF *newcurve = new QPointF[w1+w2];
for (int i = 0; i < w1; ++i) {
newcurve[i] = cu1[i];
}
for (int i = 0; i < w2; ++i) {
newcurve[i+w1] = cu2[i];
}
delete[] cu1;
delete[] cu2;
width = w1 + w2;
return newcurve;
}
void vpSystem::IntegrateDynamicsRK4(scalar time_step)
{
int i;
scalarArray q, dq, K1, K2, K3, K4, dK1, dK2, dK3, dK4;
SE3 T;
se3 TK1, TK2, TK3, TK4, V, VK1, VK2, VK3, VK4;
q.resize(m_iNumTotalDOF);
dq.resize(m_iNumTotalDOF);
K1.resize(m_iNumTotalDOF);
K2.resize(m_iNumTotalDOF);
K3.resize(m_iNumTotalDOF);
K4.resize(m_iNumTotalDOF);
dK1.resize(m_iNumTotalDOF);
dK2.resize(m_iNumTotalDOF);
dK3.resize(m_iNumTotalDOF);
dK4.resize(m_iNumTotalDOF);
ForwardDynamics();
for ( i = 0; i < m_iNumTotalDOF; i++ )
{
q[i] = m_sState[i].GetDisplacement();
dq[i] = m_sState[i].GetVelocity();
}
if ( !m_pRoot->m_bIsGround )
{
T = m_pRoot->m_sFrame;
V = m_pRoot->m_sV;
}
for ( i = 0; i < m_iNumTotalDOF; i++ )
{
K1[i] = time_step * m_sState[i].GetVelocity();
dK1[i] = time_step * m_sState[i].GetAcceleration();
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + SCALAR_1_2 * K1[i]);
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + SCALAR_1_2 * dK1[i]);
}
if ( !m_pRoot->m_bIsGround )
{
TK1 = m_pRoot->m_sV;
TK1 *= time_step;
VK1 = m_pRoot->m_sDV;
VK1 *= time_step;
m_pRoot->m_sFrame *= Exp(SCALAR_1_2 * TK1);
m_pRoot->m_sV += SCALAR_1_2 * VK1;
}
ForwardDynamics();
for ( i = 0; i < m_iNumTotalDOF; i++ )
{
K2[i] = time_step * m_sState[i].GetVelocity();
dK2[i] = time_step * m_sState[i].GetAcceleration();
m_sState[i].SetDisplacement(q[i] + SCALAR_1_2 * K2[i]);
m_sState[i].SetVelocity(dq[i] + SCALAR_1_2 * dK2[i]);
}
if ( !m_pRoot->m_bIsGround )
{
TK2 = m_pRoot->m_sV;
TK2 *= time_step;
VK2 = m_pRoot->m_sDV;
VK2 *= time_step;
m_pRoot->m_sFrame = T;
m_pRoot->m_sFrame *= Exp(SCALAR_1_2 * TK2);
m_pRoot->m_sV = V;
m_pRoot->m_sV += SCALAR_1_2 * VK2;
}
ForwardDynamics();
for ( i = 0; i < m_iNumTotalDOF; i++ )
{
K3[i] = time_step * m_sState[i].GetVelocity();
dK3[i] = time_step * m_sState[i].GetAcceleration();
m_sState[i].SetDisplacement(q[i] + K3[i]);
m_sState[i].SetVelocity(dq[i] + dK3[i]);
}
if ( !m_pRoot->m_bIsGround )
{
TK3 = m_pRoot->m_sV;
TK3 *= time_step;
VK3 = m_pRoot->m_sDV;
VK3 *= time_step;
m_pRoot->m_sFrame = T;
m_pRoot->m_sFrame *= Exp(TK3);
m_pRoot->m_sV = V;
m_pRoot->m_sV += VK3;
}
ForwardDynamics();
for ( i = 0; i < m_iNumTotalDOF; i++ )
{
K4[i] = time_step * m_sState[i].GetVelocity();
dK4[i] = time_step * m_sState[i].GetAcceleration();
m_sState[i].SetDisplacement(q[i] + SCALAR_1_6 * (K1[i] + K4[i]) + SCALAR_1_3 * (K2[i] + K3[i]));
m_sState[i].SetVelocity(dq[i] + SCALAR_1_6 * (dK1[i] + dK4[i]) + SCALAR_1_3 * (dK2[i] + dK3[i]));
}
if ( !m_pRoot->m_bIsGround )
{
TK4 = m_pRoot->m_sV;
TK4 *= time_step;
VK4 = m_pRoot->m_sDV;
VK4 *= time_step;
m_pRoot->m_sFrame = T;
TK1 *= SCALAR_1_6;
TK2 *= SCALAR_1_3;
TK3 *= SCALAR_1_3;
TK4 *= SCALAR_1_6;
TK1 += TK2;
TK1 += TK3;
TK1 += TK4;
m_pRoot->m_sFrame *= Exp(TK1);
m_pRoot->m_sV = V;
VK1 *= SCALAR_1_6;
VK2 *= SCALAR_1_3;
VK3 *= SCALAR_1_3;
VK4 *= SCALAR_1_6;
m_pRoot->m_sV += VK1;
m_pRoot->m_sV += VK2;
m_pRoot->m_sV += VK3;
m_pRoot->m_sV += VK4;
}
Reparameterize();
}
// \delta v = (I - h J_v)^{-1} (h a)
// \delta q = h (\delta v + v)
void vpSystem::IntegrateDynamicsBackwardEulerFast(scalar h)
{
if ( m_pRoot->m_bIsGround )
{
int i, j, n = m_sState.size();
if ( !n ) return;
ForwardDynamics();
RMatrix _a(n,1), _v(n,1);
RMatrix _Jv(n,n);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
RMatrix delv, A = Eye<scalar>(n) - h * _Jv;
SolveAxEqualB(A, delv, h * _a);
RMatrix delq = h * (delv + _v);
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + delv[i]);
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + delq[i]);
}
} else
{
int i, j, n = m_sState.size(), m = n + 6;
ForwardDynamics();
RMatrix _a(m,1), _v(m,1);
RMatrix _Jv(m,m);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
memcpy(&_a[n], &m_pRoot->m_sDV[0], sizeof(se3));
memcpy(&_v[n], &m_pRoot->m_sV[0], sizeof(se3));
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
m_pRoot->m_sV[i] += LIE_EPS;
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sV[i] -= LIE_EPS;
}
RMatrix delv, A = Eye<scalar>(m) - h * _Jv;
SolveAxEqualB(A, delv, h * _a);
RMatrix delq = h * (delv + _v);
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + delv[i]);
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + delq[i]);
}
for ( i = 0; i < 6; i++ ) m_pRoot->m_sV[i] += delv[n+i];
m_pRoot->m_sFrame *= Exp(se3(delq[n], delq[n+1], delq[n+2], delq[n+3], delq[n+4], delq[n+5]));
}
Reparameterize();
}
// \delta v = (I - h J_v - h^2 J_q)^{-1} (h a + h^2 J_q v)
// \delta q = h (\delta v + v)
void vpSystem::IntegrateDynamicsBackwardEuler(scalar time_step)
{
if ( m_pRoot->m_bIsGround )
{
int i, j, n = m_sState.size();
if ( !n ) return;
ForwardDynamics();
RMatrix _a(n,1), _v(n,1);
RMatrix _Jq(n,n), _Jv(n,n);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS);
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
_Jq *= (time_step * time_step);
_Jv *= -time_step;
_Jv -= _Jq;
for ( i = 0; i < n; i++ ) _Jv[i*(n+1)] += SCALAR_1;
_a *= time_step;
_a += _Jq * _v;
SolveAxEqualB_(_Jv, _a);
for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(_v[i] + _a[i]);
_a += _v;
_a *= time_step;
for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _a[i]);
} else
{
int i, j, n = m_sState.size(), m = n + 6;
ForwardDynamics();
RMatrix _a(m,1), _v(m,1), _dx(m,1);
RMatrix _Jq(m,m), _Jv(m,m), _M = Eye<scalar>(m,m);
SE3 _T;
se3 gv(SCALAR_0);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
memcpy(&_a[n], &m_pRoot->m_sDV[0], sizeof(se3));
memcpy(&_v[n], &m_pRoot->m_sV[0], sizeof(se3));
for ( i = 0; i < n; i++ )
{
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jq(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
gv[i] += LIE_EPS;
_T = m_pRoot->m_sFrame;
m_pRoot->m_sFrame *= Exp(gv);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jq(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sFrame = _T;
gv[i] = SCALAR_0;
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
m_pRoot->m_sV[i] += LIE_EPS;
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sV[i] -= LIE_EPS;
}
_Jq *= (time_step * time_step);
_Jv *= time_step;
_M -= _Jq;
_M -= _Jv;
_a *= time_step;
_a += _Jq * _v;
SolveAxEqualB(_M, _dx, _a);
for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(m_sState[i].GetVelocity() + _dx[i]);
for ( i = 0; i < 6; i++ ) m_pRoot->m_sV[i] += _dx[n+i];
_dx += _v;
_dx *= time_step;
for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _dx[i]);
m_pRoot->m_sFrame *= Exp(se3(_dx[n], _dx[n+1], _dx[n+2], _dx[n+3], _dx[n+4], _dx[n+5]));
}
Reparameterize();
}
