Skip to content
Commits on Source (2)
Hide whitespace changes
Inline Side-by-side
6_ik/IkSim.cpp
View file @ 34ea40eb
#include "IkSim.h"
void Skeleton::init() {
// left arm
Bone leftArm(Eigen::Vector3d(-1.2, 3, 0));
leftArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(-0.5, -1.7, -0.8));
leftArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -1.4, 1.4));
// right arm
Bone rightArm(Eigen::Vector3d(1.2, 3, 0));
rightArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0.5, -1.7, -0.8));
rightArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -1.4, 1.4));
// left leg
Bone leftLeg(Eigen::Vector3d(-0.7, 0, 0));
leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, 0.2));
leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, -0.4));
leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 1.2));
// right leg
Bone rightLeg(Eigen::Vector3d(0.7, 0, 0));
rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, 0.2));
rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, -0.4));
rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 1.2));
// head
Bone head(Eigen::Vector3d(0, 3, 0));
head.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0.7, 0));
m_bones.emplace_back(leftArm);
m_bones.emplace_back(rightArm);
m_bones.emplace_back(leftLeg);
m_bones.emplace_back(rightLeg);
m_bones.emplace_back(head);
}
bool IkSim::advance() {
// advance step
m_step++;
Eigen::Vector2d targetPos = m_skeleton.m_pos
+ Eigen::Vector2d(5 * sqrt(0.5), sqrt(0.5))
+ Eigen::Vector2d(sin(m_step/50.0), 0); // target position
int iterNum = 10;
float alpha = 0.1;
for(int i = 0; i < iterNum; i++){
Eigen::Vector2d currentPos = forwardKinematics();
int iterNum = 100;
float alpha = 0.01;
std::vector<Eigen::Vector3d> targetPos;
Eigen::Vector3d footPos1 = Eigen::Vector3d(0, 0, 3);
Eigen::Vector3d footPos2 = Eigen::Vector3d(0, 0, -4);
for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
Bone& bone = m_skeleton.m_bones[i];
Eigen::Vector3d endPos = m_bone_init_end[i];
switch (i) {
case 0:
endPos += Eigen::Vector3d(0, 0, sin(m_step/ 100.0 * M_PI) * 2);
break;
case 1:
endPos += Eigen::Vector3d(0, 0, -sin(m_step / 100.0 * M_PI) * 2);
break;
case 2:
endPos += Eigen::Vector3d(0, (cos((m_step / 100.0 + 1) * M_PI) + 1)/2, -sin(m_step / 100.0 * M_PI) * 2);
break;
case 3:
endPos += Eigen::Vector3d(0, (cos((m_step/100.0 + 1) * M_PI) + 1)/2, sin(m_step / 100.0 * M_PI) * 2);
break;
default:
break;
}
targetPos.push_back(endPos);
}
Eigen::Vector2d error = targetPos - currentPos;
//m_skeleton.base_pos += Eigen::Vector3d(0, 0, m_step / 1000.0);
double errorNorm = error.norm();
for(int iter = 0; iter < iterNum; iter++){
for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
Bone& bone = m_skeleton.m_bones[i];
Eigen::Vector3d endPos = bone.forwardKinematics(bone.m_num_joints - 1);
if (errorNorm < 0.01) {
return false; // stop if the error is small enough
}
Eigen::Vector3d error = targetPos[i] - endPos;
// construct the Jacobian matrix
Eigen::MatrixXd J = computeJacobian(targetPos);
double errorNorm = error.norm();
auto deltaTheta = alpha * J.transpose() * error;
// construct the Jacobian matrix
Eigen::MatrixXd J = bone.computeJacobian(endPos);
for (int i = 0; i < m_skeleton.m_num_bones; i++) {
m_skeleton.m_bones[i].angle += deltaTheta(i, 0);
}
}
auto deltaTheta = alpha * J.transpose() * error;
for (int j = 0; j < bone.m_num_joints; j++) {
bone.m_joints[j].angles += deltaTheta.segment(j * 3, 3);
}
}
}
return false;
}
Eigen::MatrixXd IkSim::computeJacobian(Eigen::Vector2d targetPos) {
Eigen::MatrixXd J(2, m_skeleton.m_num_bones);
Eigen::MatrixXd Bone::computeJacobian(Eigen::Vector3d endPos) {
Eigen::MatrixXd J(3, m_num_joints * 3);
// compute the Jacobian matrix
double accumAngle = 0;
for (int i = 0; i < m_skeleton.m_num_bones; i++) {
accumAngle += m_skeleton.m_bones[i].angle;
}
double accumX = 0;
double accumY = 0;
for (int i = m_skeleton.m_num_bones - 1; i >= 0; --i) {
double angle = m_skeleton.m_bones[i].angle;
double length = m_skeleton.m_bones[i].length;
double dx = length * cos(accumAngle);
double dy = length * sin(accumAngle);
J(0, i) = -dy; // partial derivative with respect to angle
J(1, i) = dx; // partial derivative with respect to length
accumX += dx;
accumY += dy;
accumAngle -= angle;
Eigen::Matrix3d rotate = Eigen::Matrix3d::Identity();
for (int i = 0; i < m_num_joints; i++) {
Eigen::Matrix3d jointRotate = m_joints[i].toRotation();
rotate = jointRotate * rotate;
Eigen::Vector3d jointPos = forwardKinematics(i - 1);
Eigen::Vector3d p = endPos - jointPos;
Eigen::Vector3d xAxis = rotate.col(0);
Eigen::Vector3d yAxis = rotate.col(1);
Eigen::Vector3d zAxis = rotate.col(2);
// fill the three columns of the Jacobian matrix, each column is a 3D vector
J.col(i * 3) = xAxis.cross(p);
J.col(i * 3 + 1) = yAxis.cross(p);
J.col(i * 3 + 2) = zAxis.cross(p);
}
return J;
}
......@@ -64,21 +125,19 @@ Eigen::VectorXd IkSim::computeGradient() {
return grad;
}
Eigen::Vector2d IkSim::forwardKinematics() {
double accumX = 0;
double accumY = 0;
double accumAngle = 0;
for (int i = 0; i < m_skeleton.m_num_bones; i++) {
Bone& b = m_skeleton.m_bones[i];
accumAngle += b.angle;
double dx = b.length * cos(accumAngle);
double dy = b.length * sin(accumAngle);
Eigen::Vector3d Bone::forwardKinematics(int joint_index) {
if (joint_index < -1 || joint_index >= m_num_joints) {
printf("joint index out of range\n");
exit(-1);
}
Eigen::Vector3d pos = m_root;
Eigen::Matrix3d rotate = Eigen::Matrix3d::Identity();
accumX += dx;
accumY += dy;
}
for (int i = 0; i <= joint_index; i++) {
Eigen::Matrix3d jointRotate = m_joints[i].toRotation();
rotate = jointRotate * rotate;
pos += rotate * m_joints[i].extent;
}
return Eigen::Vector2d(accumX + m_skeleton.m_pos.x(), accumY + m_skeleton.m_pos.y());
return pos;
}
\ No newline at end of file
6_ik/IkSim.h
View file @ 34ea40eb
......@@ -2,35 +2,70 @@
#include <time.h>
using namespace std;
struct Bone {
Bone(double l, double a) : length(l), angle(a) {}
~Bone() {}
double length;
double angle;
inline float getRandomFloat(float min, float max) {
return min + static_cast<float>(rand()) / (static_cast<float>(RAND_MAX / (max - min)));
}
struct Joint {
Eigen::Vector3d angles; // euler angles in degrees
Eigen::Vector3d extent; // length and direction of the bone
Eigen::Matrix3d toRotation() {
Eigen::Matrix3d rotation;
rotation = Eigen::AngleAxisd(angles[2] * M_PI / 180, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(angles[1] * M_PI / 180, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(angles[0] * M_PI / 180, Eigen::Vector3d::UnitX());
return rotation;
}
Joint() : angles(Eigen::Vector3d(0, 0, 0)), extent(Eigen::Vector3d(1, 0, 0)) {} // default constructor
Joint(Eigen::Vector3d angles, Eigen::Vector3d extent) : angles(angles), extent(extent) {}
~Joint() {}
};
struct Skeleton {
Skeleton(): m_pos(Eigen::Vector2d(-2.5, 0)) {}
~Skeleton() {}
void init(int num_bones) {
struct Bone {
Bone(): m_root(Eigen::Vector3d(0, 0, 0)) {}
Bone(Eigen::Vector3d root) : m_root(root) {}
~Bone() {}
void init(int num_joints) {
srand(time(NULL));
m_num_bones = num_bones;
m_bones.clear();
m_bones.reserve(num_bones);
for (int i = 0; i < num_bones; ++i) {
double length = 1.0; // default length
double angle = (rand() / (RAND_MAX + 1.0) * 2 - 1) * M_PI / 3; // random angle between [-pi, pi]
m_num_joints = num_joints;
m_joints.clear();
m_joints.reserve(num_joints);
for (int i = 0; i < num_joints; ++i) {
Eigen::Vector3d angles = Eigen::Vector3d(0, 0, 90 - (i % 2) * 180);
if (i == 0)
angle = M_PI / 4;
else
angle = M_PI / 2 * (1 - ((i % 2) * 2));
m_bones.emplace_back(length, angle);
angles = Eigen::Vector3d(0, 0, 45);
else
angles = Eigen::Vector3d(0, 0, -45);
Eigen::Vector3d extent = Eigen::Vector3d(1, 0, 0);
m_joints.emplace_back(angles, extent);
}
}
int m_num_bones; // number of bones in the skeleton
Eigen::Vector2d m_pos; // position of the root joint
std::vector<Bone> m_bones; // bones of the skeleton
int m_num_joints = 0; // number of joints in the bone
Eigen::Vector3d m_root; // root position of the bone
std::vector<Joint> m_joints; // joints of the bone
Eigen::Vector3d forwardKinematics(int joint_index);
Eigen::MatrixXd computeJacobian(Eigen::Vector3d endPos);
void addJoint(Eigen::Vector3d angles, Eigen::Vector3d extent) {
m_joints.emplace_back(angles, extent);
m_num_joints++;
}
};
struct Skeleton {
Eigen::Vector3d base_pos = Eigen::Vector3d::Zero();
std::vector<Bone> m_bones; // bones of the skeleton
Skeleton(){}
~Skeleton(){}
void init();
void addBone(Bone& bone) {
m_bones.emplace_back(bone);
}
Eigen::Vector3d forwardKinematics(Bone& bone, int joint_index);
};
class IkSim : public Simulation {
......@@ -40,18 +75,33 @@ public:
}
virtual void init() override {
m_skeleton.init(5); // Initialize skeleton with 5 bones
m_skeleton.base_pos = Eigen::Vector3d(0, 0, 0); // Initialize root position
m_skeleton.init(); // Initialize skeleton with 2 joints
std::string path = "bone.off";
initEndVector(); // Initialize end positions of bones
std::string path = "cube.off";
m_objects.clear();
for (int i = 0; i < m_skeleton.m_num_bones; i++) {
m_objects.push_back(RigidObject(path));
m_objects[i].setScale(0.125);
}
//for (int i = 0; i < m_skeleton.m_num_joints; i++) {
// m_objects.push_back(RigidObject(path));
// m_objects[i].setScale(0.5, 0.05, 0.05);
//}
for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
Bone& bone = m_skeleton.m_bones[i];
for (int j = 0; j < bone.m_num_joints; j++) {
m_objects.emplace_back(path);
m_objects.back().setScale(bone.m_joints[j].extent.norm()/2, 0.2, 0.2);
}
}
m_objects.emplace_back(path);
m_objects.back().setScale(1, 1.5, 0.5);
m_objects.back().setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 1.5, 0));
m_objects.push_back(RigidObject("sphere.off"));
m_objects[m_skeleton.m_num_bones].setScale(0.01);
m_objects[m_skeleton.m_num_bones].setPosition(Eigen::Vector3d(-2.5, 0, 0));
m_objects.emplace_back("sphere.off");
m_objects.back().setScale(0.04);
m_objects.back().setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 4, 0));
m_dt = 5e-2;
m_radius = 10;
......@@ -59,40 +109,61 @@ public:
reset();
}
void initEndVector() {
for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
Bone& bone = m_skeleton.m_bones[i];
Eigen::Vector3d endPos = bone.forwardKinematics(bone.m_num_joints - 1);
m_bone_init_end.push_back(endPos);
}
}
virtual void resetMembers() override {
}
void updateSkeletonPosition() {
double accumX = 0;
double accumY = 0;
double accumAngle = 0;
for (int i = 0; i < m_skeleton.m_num_bones; i++) {
RigidObject& o = m_objects[i];
Bone& b = m_skeleton.m_bones[i];
accumAngle += b.angle;
double dx = b.length * cos(accumAngle);
double dy = b.length * sin(accumAngle);
o.setPosition(Eigen::Vector3d(m_skeleton.m_pos.x() + accumX + dx / 2, m_skeleton.m_pos.y() + accumY + dy / 2, 0));
int rigidIndex = 0;
for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
Bone& bone = m_skeleton.m_bones[i];
Eigen::Vector3d pos = m_skeleton.base_pos + bone.m_root;
Eigen::Matrix3d rotate = Eigen::Matrix3d::Identity();
for (int j = 0; j < bone.m_num_joints; j++) {
RigidObject& o = m_objects[rigidIndex];
Joint& b = bone.m_joints[j];
Eigen::Matrix3d jointRotate = bone.m_joints[j].toRotation();
rotate = jointRotate * rotate;
Eigen::Vector3d extent = rotate * b.extent;
o.setPosition(pos + extent / 2);
Eigen::Vector3d unit = Eigen::Vector3d(1, 0, 0);
Eigen::Vector3d normalExtent = b.extent.normalized();
Eigen::Vector3d axis = unit.cross(normalExtent);
double angle = acos(unit.dot(normalExtent));
Eigen::Matrix3d extentRotate = Eigen::Matrix3d::Identity();
if (axis.norm() > 1e-6) {
axis.normalize();
extentRotate = Eigen::AngleAxisd(angle, axis).toRotationMatrix();
}
o.setRotation(rotate * extentRotate);
Eigen::Matrix3d rotate = Eigen::AngleAxisd(accumAngle, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();
Eigen::Matrix3d rotateInv = rotate.inverse();
pos += extent;
o.setRotation(rotate);
accumX += dx;
accumY += dy;
rigidIndex++;
}
}
}
m_objects[rigidIndex].setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 1.5, 0));
Eigen::MatrixXd computeJacobian(Eigen::Vector2d targetPos);
m_objects[rigidIndex + 1].setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 4, 0));
}
Eigen::VectorXd computeGradient();
Eigen::Vector2d forwardKinematics();
virtual void updateRenderGeometry() override {
updateSkeletonPosition();
for (size_t i = 0; i < m_objects.size(); i++) {
......@@ -141,7 +212,8 @@ public:
private:
float m_radius;
Skeleton m_skeleton;
std::vector<Eigen::Vector3d> m_bone_init_end; // initial end positions of bones
std::vector<Eigen::MatrixXd> m_renderVs; // vertex positions for rendering
std::vector<Eigen::MatrixXi> m_renderFs; // face indices for rendering
};
\ No newline at end of file
include/BaseObject.cpp
View file @ 34ea40eb
......@@ -60,7 +60,11 @@ void BaseObject::recomputeCOM() {
m_mesh.V = m_mesh.V.rowwise() - COM.transpose();
}
void BaseObject::setScale(double s) { m_scale = s; }
void BaseObject::setScale(Eigen::Vector3d s) { m_scale = s; }
void BaseObject::setScale(double s) { m_scale = Eigen::Vector3d(s, s, s); }
void BaseObject::setScale(double sX, double sY, double sZ) { m_scale = Eigen::Vector3d(sX, sY, sZ); }
void BaseObject::setID(int id) { m_id = id; }
......@@ -80,7 +84,7 @@ void BaseObject::setRotation(const Eigen::Matrix3d& R) {
void BaseObject::setColors(const Eigen::MatrixXd& C) { m_mesh.C = C; }
double BaseObject::getScale() const { return m_scale; }
Eigen::Vector3d BaseObject::getScale() const { return m_scale; }
int BaseObject::getID() const { return m_id; }
......@@ -93,14 +97,14 @@ Eigen::Quaterniond BaseObject::getRotation() const { return m_quat; }
Eigen::Matrix3d BaseObject::getRotationMatrix() const { return m_rot; }
Eigen::Vector3d BaseObject::getVertexPosition(int vertexIndex) const {
return m_mesh.V.row(vertexIndex) * m_scale *
return m_mesh.V.row(vertexIndex).cwiseProduct(m_scale.transpose()) *
getRotationMatrix().transpose() +
getPosition().transpose();
}
void BaseObject::getMesh(Eigen::MatrixXd& V, Eigen::MatrixXi& F) const {
// get mesh after rotation and translation
V = (m_mesh.V * m_scale * getRotationMatrix().transpose()).rowwise() +
V = (m_mesh.V* m_scale.asDiagonal() * getRotationMatrix().transpose()).rowwise() +
getPosition().transpose();
F = m_mesh.F;
}
......
include/BaseObject.h
View file @ 34ea40eb
......@@ -35,7 +35,9 @@ public:
void reset();
void recomputeCOM();
void setScale(Eigen::Vector3d s);
void setScale(double s);
void setScale(double sX, double sY, double sZ);
void setID(int id);
virtual void setType(ObjType t);
void setPosition(const Eigen::Vector3d& p);
......@@ -43,7 +45,7 @@ public:
void setRotation(const Eigen::Matrix3d& R);
void setColors(const Eigen::MatrixXd& C);
double getScale() const;
Eigen::Vector3d getScale() const;
int getID() const;
ObjType getType() const;
Eigen::Vector3d getPosition() const;
......@@ -65,7 +67,7 @@ protected:
Mesh m_mesh;
ObjType m_type;
double m_scale = 1.0; // Scale
Eigen::Vector3d m_scale = Eigen::Vector3d(1.0, 1.0, 1.0); // Scale
Eigen::Vector3d m_position; // Position of the center of mass
Eigen::Quaterniond m_quat; // Rotation (quaternion)
Eigen::Matrix3d m_rot; // Rotation (matrix)
......