6_ik/IkSim.cpp

 #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

@@ -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

@@ -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

@@ -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)