void trackball::
rotate(double fx, double fy, double tx, double ty) {
scm::math::vec3d start(fx, fy, project_to_sphere(fx, fy));
scm::math::vec3d end(tx, ty, project_to_sphere(tx, ty));
scm::math::vec3d diff(end - start);
double diff_len = scm::math::length(diff);
scm::math::vec3d rot_axis(cross(start, end));
double rot_angle = 2.0 * asin(scm::math::clamp(diff_len/(2.0 * radius_), -1.0, 1.0));
scm::math::mat4d tmp(scm::math::mat4d::identity());
scm::math::mat4d tmp_dolly(scm::math::mat4d::identity());
scm::math::mat4d tmp_dolly_inv(scm::math::mat4d::identity());
scm::math::translate(tmp_dolly, 0.0, 0.0, dolly_);
scm::math::translate(tmp_dolly_inv, 0.0, 0.0, -dolly_);
scm::math::rotate(tmp, scm::math::rad2deg(rot_angle), rot_axis);
transform_ = tmp_dolly * tmp * tmp_dolly_inv * transform_;
}
geometry_msgs::Pose C_HunoLimbKinematics::ForwardKinematics(const double *thetas_C)
{
geometry_msgs::Pose outLimbPose;
Eigen::Vector3d rot_axis = Eigen::Vector3d::Zero();
double rot_angle;
Eigen::Matrix4d limbTF = Eigen::Matrix4d::Identity();
Eigen::Matrix4d expXihatTheta = Eigen::Matrix4d::Identity();
Eigen::Matrix<double, 6, 5> tempJacobian;
tempJacobian.setZero(); // initialize temporary variable to zeros
int limbCtr = 0;
/* Lock jacobian matrix while being formed */
m_isJacobianLocked = true;
/* Calculate joint transformation matrices sequentially and populate jacobian */
for (int jointIdx = 0; jointIdx < m_numJoints; jointIdx++)
{
/* use previous expXihatTheta to generate adjoint matrix
and fill next column of jacobian */
tempJacobian.col(limbCtr) = AdjointMatrix(expXihatTheta) * m_zetas_M.col(jointIdx);
/* calculate for next joint */
expXihatTheta = ExpXihatTheta(jointIdx, (*(thetas_C+jointIdx))*DEG2RAD);
limbTF *= expXihatTheta;
/* increment counter */
limbCtr++;
}
if (limbCtr == m_numJoints)
{
limbTF *= m_g0Mat_M; // apply configuration matrix
m_jacobian_M = tempJacobian.block(0,0, m_numJoints, m_numJoints); // save jacobian
m_isJacobianLocked = false; // unlock jacobian matrix
}
else
{ // reset matrices since something didn't add up.
limbTF.setZero();
m_jacobian_M.setZero();
}
/* save limb transformation matrix into pose message */
outLimbPose.position.x = limbTF(0,3);
outLimbPose.position.y = limbTF(1,3);
outLimbPose.position.z = limbTF(2,3);
//back out rotation unit vector and angle from rotation matrix
rot_angle = acos( ( ((limbTF.block<3,3>(0,0)).trace())-1 ) /2 );
if (sin(rot_angle) != 0)
{
rot_axis(0) = (limbTF(2,1)-limbTF(1,2)) / (2*sin(rot_angle));
rot_axis(1) = (limbTF(0,2)-limbTF(2,0)) / (2*sin(rot_angle));
rot_axis(2) = (limbTF(1,0)-limbTF(0,1)) / (2*sin(rot_angle));
}
// else rot_axis is zeroes
outLimbPose.orientation.x = rot_axis(0)*sin(rot_angle/2);
outLimbPose.orientation.y = rot_axis(1)*sin(rot_angle/2);
outLimbPose.orientation.z = rot_axis(2)*sin(rot_angle/2);
outLimbPose.orientation.w = cos(rot_angle/2);
return outLimbPose;
} // end ForwardKinematics()
void GripperSelTransform::MakeMatrix ( const double* from, const double* to, const double* object_m, gp_Trsf& mat )
{
mat = gp_Trsf();
switch ( m_data.m_type )
{
case GripperTypeTranslate:
mat.SetTranslation ( gp_Vec ( make_point ( from ), make_point ( to ) ) );
break;
case GripperTypeScale:
{
gp_Trsf object_mat = make_matrix(object_m);
gp_Pnt scale_centre_point = gp_Pnt(0, 0, 0).Transformed(object_mat);
double dist = make_point ( from ).Distance ( scale_centre_point );
if ( dist<0.00000001 )
{
return;
}
double scale = make_point ( to ).Distance ( scale_centre_point ) /dist;
mat.SetScale( scale_centre_point, scale );
}
break;
case GripperTypeObjectScaleX:
{
gp_Trsf object_mat = make_matrix(object_m);
gp_Vec object_x = gp_Vec(1, 0, 0).Transformed(object_mat).Normalized();
gp_Pnt scale_centre_point = gp_Pnt(0, 0, 0).Transformed(object_mat);
double old_x = make_vector(from) * object_x - gp_Vec(scale_centre_point.XYZ()) * object_x;
double new_x = make_vector(to) * object_x - gp_Vec(scale_centre_point.XYZ()) * object_x;
if(fabs(old_x) < 0.000000001)return;
double scale = new_x/old_x;
double m[16] = {scale, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
mat = object_mat * make_matrix(m) * object_mat.Inverted();
}
break;
case GripperTypeObjectScaleY:
{
gp_Trsf object_mat = make_matrix(object_m);
gp_Vec object_y = gp_Vec(0, 1, 0).Transformed(object_mat).Normalized();
gp_Pnt scale_centre_point = gp_Pnt(0, 0, 0).Transformed(object_mat);
double old_y = make_vector(from) * object_y - gp_Vec(scale_centre_point.XYZ()) * object_y;
double new_y = make_vector(to) * object_y - gp_Vec(scale_centre_point.XYZ()) * object_y;
if(fabs(old_y) < 0.000000001)return;
double scale = new_y/old_y;
double m[16] = {1, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
mat = object_mat * make_matrix(m) * object_mat.Inverted();
}
break;
case GripperTypeObjectScaleZ:
{
gp_Trsf object_mat = make_matrix(object_m);
gp_Vec object_z = gp_Vec(0, 0, 1).Transformed(object_mat).Normalized();
gp_Pnt scale_centre_point = gp_Pnt(0, 0, 0).Transformed(object_mat);
double old_z = make_vector(from) * object_z - gp_Vec(scale_centre_point.XYZ()) * object_z;
double new_z = make_vector(to) * object_z - gp_Vec(scale_centre_point.XYZ()) * object_z;
if(fabs(old_z) < 0.000000001)return;
double scale = new_z/old_z;
double m[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1};
mat = object_mat * make_matrix(m) * object_mat.Inverted();
}
break;
case GripperTypeObjectScaleXY:
{
gp_Trsf object_mat = make_matrix(object_m);
gp_Vec object_x = gp_Vec(1, 0, 0).Transformed(object_mat).Normalized();
gp_Pnt scale_centre_point = gp_Pnt(0, 0, 0).Transformed(object_mat);
double old_x = make_vector(from) * object_x - gp_Vec(scale_centre_point.XYZ()) * object_x;
double new_x = make_vector(to) * object_x - gp_Vec(scale_centre_point.XYZ()) * object_x;
if(fabs(old_x) < 0.000000001)return;
double scale = new_x/old_x;
double m[16] = {scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale};
mat = object_mat * make_matrix(m) * object_mat.Inverted();
}
break;
case GripperTypeRotate:
case GripperTypeRotateObject:
case GripperTypeRotateObjectXY:
case GripperTypeRotateObjectXZ:
case GripperTypeRotateObjectYZ:
{
gp_Trsf object_mat = make_matrix(object_m);
gp_Pnt rotate_centre_point = gp_Pnt(0, 0, 0).Transformed(object_mat);
gp_Vec start_to_end_vector(make_point(from), make_point(to));
if ( start_to_end_vector.Magnitude() <0.000001 ) return;
gp_Vec start_vector ( rotate_centre_point, make_point ( from ) );
gp_Vec end_vector ( rotate_centre_point, make_point ( to ) );
if ( start_vector.Magnitude() <0.000001 ) return;
if ( end_vector.Magnitude() <0.000001 ) return;
mat.SetTranslation ( -gp_Vec ( rotate_centre_point.XYZ() ) );
gp_Vec vx, vy;
wxGetApp().m_current_viewport->m_view_point.GetTwoAxes(vx, vy, false, 0);
gp_Vec rot_dir = vx ^ vy;
rot_dir.Normalize();
if(m_data.m_type == GripperTypeRotateObjectXY){
// use object z axis
gp_Vec object_x = gp_Vec(1, 0, 0).Transformed(object_mat).Normalized();
gp_Vec object_y = gp_Vec(0, 1, 0).Transformed(object_mat).Normalized();
gp_Vec object_z = gp_Vec(0, 0, 1).Transformed(object_mat).Normalized();
rot_dir = object_z;
vx = object_x;
vy = object_y;
}
else if(m_data.m_type == GripperTypeRotateObjectXZ){
// use object y axis
gp_Vec object_x = gp_Vec(1, 0, 0).Transformed(object_mat).Normalized();
gp_Vec object_y = gp_Vec(0, 1, 0).Transformed(object_mat).Normalized();
gp_Vec object_z = gp_Vec(0, 0, 1).Transformed(object_mat).Normalized();
rot_dir = object_y;
vx = object_z;
vy = object_x;
}
else if(m_data.m_type == GripperTypeRotateObjectYZ){
// use object x axis
gp_Vec object_x = gp_Vec(1, 0, 0).Transformed(object_mat).Normalized();
gp_Vec object_y = gp_Vec(0, 1, 0).Transformed(object_mat).Normalized();
gp_Vec object_z = gp_Vec(0, 0, 1).Transformed(object_mat).Normalized();
rot_dir = object_x;
vx = object_y;
vy = object_z;
}
else if(m_data.m_type == GripperTypeRotateObject){
// choose the closest object axis to use
gp_Vec object_x = gp_Vec(1, 0, 0).Transformed(object_mat).Normalized();
gp_Vec object_y = gp_Vec(0, 1, 0).Transformed(object_mat).Normalized();
gp_Vec object_z = gp_Vec(0, 0, 1).Transformed(object_mat).Normalized();
double dpx = fabs(rot_dir * object_x);
double dpy = fabs(rot_dir * object_y);
double dpz = fabs(rot_dir * object_z);
if(dpx > dpy && dpx > dpz){
// use object x axis
rot_dir = object_x;
vx = object_y;
vy = object_z;
}
else if(dpy > dpz){
// use object y axis
rot_dir = object_y;
vx = object_z;
vy = object_x;
}
else{
// use object z axis
rot_dir = object_z;
vx = object_x;
vy = object_y;
}
}
gp_Ax1 rot_axis(rotate_centre_point, rot_dir);
double sx = start_vector * vx;
double sy = start_vector * vy;
double ex = end_vector * vx;
double ey = end_vector * vy;
double angle = gp_Vec(sx, sy, 0).AngleWithRef(gp_Vec(ex, ey, 0), gp_Vec(0,0,1));
mat.SetRotation(rot_axis, angle);
}
break;
default:
break;
}
}
