Eigen::MatrixXd cIKSolver::BuildConsPosJacob(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const tConsDesc& cons_desc)
{
assert(static_cast<int>(cons_desc(eConsDescType)) == eConsTypePos);
int num_joints = static_cast<int>(joint_mat.rows());
int parent_id = static_cast<int>(cons_desc(eConsDescParam0));
tVector attach_pt = tVector(cons_desc(eConsDescParam1), cons_desc(eConsDescParam2), 0.f, 0.f);
tVector end_pos = cKinTree::LocalToWorldPos(joint_mat, pose, parent_id, attach_pt);
const Eigen::Vector3d rot_axis = Eigen::Vector3d(0, 0, 1);
int num_dof = cKinTree::GetNumDof(joint_mat);
Eigen::MatrixXd J = Eigen::MatrixXd(gPosDims, num_dof);
J.setZero();
for (int i = 0; i < gPosDims; ++i)
{
J(i, i) = 1;
}
int curr_id = parent_id;
while (true)
{
tVector joint_pos = cKinTree::CalcJointWorldPos(joint_mat, pose, curr_id);
tVector delta = end_pos - joint_pos;
Eigen::Vector3d tangent = rot_axis.cross(Eigen::Vector3d(delta(0), delta(1), delta(2)));
int curr_parent_id = cKinTree::GetParent(joint_mat, curr_id);
for (int i = 0; i < gPosDims; ++i)
{
J(i, gPosDims + curr_id) = tangent(i);
}
if (curr_parent_id == cKinTree::gInvalidJointID)
{
// no scaling for root
break;
}
else
{
#if !defined(DISABLE_LINK_SCALE)
double attach_x = joint_desc(curr_id, cKinTree::eJointDescAttachX);
double attach_y = joint_desc(curr_id, cKinTree::eJointDescAttachY);
double attach_z = joint_desc(curr_id, cKinTree::eJointDescAttachZ);
double parent_world_theta = cKinTree::CalcJointWorldTheta(joint_desc, curr_parent_id);
double world_attach_x = std::cos(parent_world_theta) * attach_x - std::sin(parent_world_theta) * attach_y;
double world_attach_y = std::sin(parent_world_theta) * attach_x + std::cos(parent_world_theta) * attach_y;
double world_attach_z = attach_z; // hack ignoring z, do this properly
J(0, gPosDims + num_joints + curr_id) = world_attach_x;
J(1, gPosDims + num_joints + curr_id) = world_attach_y;
#endif
curr_id = curr_parent_id;
}
}
return J;
}
std::shared_ptr<Joint> Joints::create_joint(PhysicsWorld &phys_world, Body &bodyA, Body &bodyB, int type)
{
//Get the Physics Context
PhysicsContext pc = phys_world.get_pc();
switch(type)
{
default:
case 0: // Distance joint
{
DistanceJointDescription joint_desc(phys_world);
joint_desc.set_bodies(bodyA, bodyB, bodyA.get_position(), bodyB.get_position());
joint_desc.set_damping_ratio(1.0f);
joint_desc.set_length(100.0f);
std::shared_ptr<Joint> joint( static_cast<Joint *> (new DistanceJoint(pc, joint_desc)));
return joint;
}
case 1: // Revolute joint
{
RevoluteJointDescription joint_desc(phys_world);
joint_desc.set_bodies(bodyA, bodyB, bodyA.get_position());
joint_desc.set_as_motor();
joint_desc.set_motor_speed(Angle(60,angle_degrees));
joint_desc.set_max_motor_torque(1000);
std::shared_ptr<Joint> joint( static_cast<Joint *> (new RevoluteJoint(pc, joint_desc)));
return joint;
}
case 2: // Prismatic joint
{
PrismaticJointDescription joint_desc(phys_world);
joint_desc.set_bodies(bodyA, bodyB, bodyA.get_position(), bodyB.get_position());
joint_desc.set_as_motor();
joint_desc.enable_limit();
joint_desc.set_axis_a(Vec2f(0.0f,1.0f));
joint_desc.set_motor_speed(Angle(20,angle_degrees));
joint_desc.set_translation_limits(10.0f,100.0f);
joint_desc.set_max_motor_force(1000);
std::shared_ptr<Joint> joint( static_cast<Joint *> (new PrismaticJoint(pc, joint_desc)));
return joint;
}
}
}
