/// Determines (and sets) the value of Q from the axis and the inboard link and outboard link transforms
void ScrewJoint::determine_q(VectorN& q)
{
// get the inboard and outboard link pointers
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
// verify that the inboard and outboard links are set
if (!inboard || !outboard)
{
std::cerr << "ScrewJoint::determine_Q() called on NULL inboard and/or outboard links!" << std::endl;
assert(false);
return;
}
// if axis is not defined, can't use this method
if (std::fabs(_u.norm() - 1.0) > NEAR_ZERO)
{
std::cerr << "ScrewJoint::determine_Q() warning: some axes undefined; aborting..." << std::endl;
return;
}
// get the attachment points on the link (global coords)
Vector3d p1 = get_position_global(false);
Vector3d p2 = get_position_global(true);
// get the joint axis in the global frame
Vector3d ug = inboard->get_transform().mult_vector(_u);
// now, we'll project p2 onto the axis ug; points will be setup so that
// ug passes through origin on inboard
q.resize(num_dof());
q[DOF_1] = ug.dot(p2-p1)/_pitch;
}
bool PathLCPSolver::solve_lcp(const MatrixN& MM, const VectorN& q, VectorN& z, double tol)
{
const Real INF = std::numeric_limits<Real>::max();
int i, j, k;
int variables;
shared_array<int> m_i, m_j;
shared_array<double> dq, lb, ub, m_ij, x_end;
MCP_Termination termination;
Output_Printf(Output_Log | Output_Status | Output_Listing, "%s: Standalone-C Link\n", Path_Version());
// We need a sparse representation of the matrix
assert(q.size() == MM.rows());
assert(q.size() == z.size());
// Copy q to dq
variables = q.size();
dq = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
dq[i] = q[i];
// Copy q to dq
variables = q.size();
dq = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
dq[i] = q[i];
int num_non_zeroes = 0;
for (i = 0;i < variables; i++)
for (j = 0;j < variables; j++)
if (std::fabs(MM(i,j)) > NEAR_ZERO)
num_non_zeroes++;
m_i = shared_array<int>(new int[num_non_zeroes + 1]);
m_j = shared_array<int>(new int[num_non_zeroes + 1]);
m_ij = shared_array<double>(new double[num_non_zeroes + 1]);
k = 0;
for (i = 0;i < variables; i++)
{
for (j = 0;j < variables; j++)
{
if (std::fabs(MM(i,j)) > NEAR_ZERO)
{
m_i[k] = i+1;
m_j[k] = j+1;
m_ij[k] = MM(i,j);
k++;
}
}
}
lb = shared_array<double>(new double[variables+1]);
ub = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
{
lb[i] = 0.0;
ub[i] = INF;
}
x_end = shared_array<double>(new double[variables+1]);
SimpleLCP(variables, num_non_zeroes, m_i.get(), m_j.get(), m_ij.get(), dq.get(), lb.get(), ub.get(), &termination, x_end.get(), tol);
// We now copy x_end into z
z.resize(variables);
for (i = 0;i < variables; i++)
z[i] = x_end[i];
return (termination == MCP_Solved);
}
/// Determines (and sets) the value of Q from the axes and the inboard link and outboard link transforms
void SphericalJoint::determine_q(VectorN& q)
{
const unsigned X = 0, Y = 1, Z = 2;
// get the inboard and outboard links
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
// verify that the inboard and outboard links are set
if (!inboard || !outboard)
throw NullPointerException("SphericalJoint::determine_q() called on NULL inboard and/or outboard links!");
// if any of the axes are not defined, can't use this method
if (std::fabs(_u[0].norm_sq() - 1.0) > NEAR_ZERO ||
std::fabs(_u[1].norm_sq() - 1.0) > NEAR_ZERO ||
std::fabs(_u[2].norm_sq() - 1.0) > NEAR_ZERO)
return;
// set proper size for q
q.resize(num_dof());
// get the link transforms
Matrix3 R_inboard, R_outboard;
inboard->get_transform().get_rotation(&R_inboard);
outboard->get_transform().get_rotation(&R_outboard);
// determine the joint transformation
Matrix3 R_local = R_inboard.transpose_mult(R_outboard);
// back out the transformation to z-axis
Matrix3 RU = _R.transpose_mult(R_local * _R);
// determine cos and sin values for q1, q2, and q3
Real s2 = RU(X,Z);
Real c2 = std::cos(std::asin(s2));
Real s1, c1, s3, c3;
if (std::fabs(c2) > NEAR_ZERO)
{
s1 = -RU(Y,Z)/c2;
c1 = RU(Z,Z)/c2;
s3 = -RU(X,Y)/c2;
c3 = RU(X,X)/c2;
assert(!std::isnan(s1));
assert(!std::isnan(c1));
assert(!std::isnan(s3));
assert(!std::isnan(c3));
}
else
{
// singular, we can pick any value for s1, c1, s3, c3 as long as the
// following conditions are satisfied
// c1*s3 + s1*c3*s2 = RU(Y,X)
// c1*c3 - s1*s3*s2 = RU(Y,Y)
// s1*s3 - c1*c3*s2 = RU(Z,X)
// s1*c3 + c1*s3*s2 = RU(Z,Y)
// so, we'll set q1 to zero (arbitrarily) and obtain
s1 = 0;
c1 = 1;
s3 = RU(Y,X);
c3 = RU(Y,Y);
}
// now determine q; only q2 can be determined without ambiguity
if (std::fabs(s1) < NEAR_ZERO)
q[DOF_2] = std::atan2(RU(X,Z), RU(Z,Z)/c1);
else
q[DOF_2] = std::atan2(RU(X,Z), -RU(Y,Z)/s1);
assert(!std::isnan(q[DOF_2]));
// if cos(q2) is not singular, proceed easily from here..
if (std::fabs(c2) > NEAR_ZERO)
{
q[DOF_1] = std::atan2(-RU(Y,Z)/c2, RU(Z,Z)/c2);
q[DOF_3] = std::atan2(-RU(X,Y)/c2, RU(X,X)/c2);
assert(!std::isnan(q[DOF_1]));
assert(!std::isnan(q[DOF_3]));
}
else
{
if (std::fabs(c1) > NEAR_ZERO)
q[DOF_3] = std::atan2((RU(Y,X) - s1*s2*c3)/c1, (RU(Y,Y) + s1*s2*s3)/c1);
else
q[DOF_3] = std::atan2((RU(Z,X) + c1*s2*c3)/s1, (RU(Z,Y) - c1*s2*s3)/s1);
if (std::fabs(c3) > NEAR_ZERO)
q[DOF_1] = std::atan2((RU(Y,X) - c1*s3)/(s2*c3), (-RU(Y,X) + s1*s3)/(s2*c3));
else
q[DOF_1] = std::atan2((-RU(Y,Y) + c1*c3)/(s2*s3), (RU(Z,Y) - s1*c3)/(s2*s3));
assert(!std::isnan(q[DOF_1]));
assert(!std::isnan(q[DOF_3]));
}
}
