0
$\begingroup$

I am using glm library and OpenGl for this. This is might not be directly related to Robotics but I guess Inverse kinematics is used there

I have 3 joints

enter image description here

Here is my code

glm::mat4 m_yellow_mat;
glm::mat4 m_pink_mat;
glm::mat4 m_blue_mat;
glm::mat4 m_red_mat;

glm::vec3 yellow_position_end;
glm::vec3 blue_position_end;
glm::vec3 pink_position_end;
glm::vec3 red_position_end;
glm::mat4 jacobian_matrix[2];
glm::vec4 anglesVector;
glm::vec4 goalVector;
float angles[9];
bool positionChanged, stop;
glm::vec3 lastTargetPosition, goal, v1, v2, v3, target;



void Bone_Animation::update(float delta_time)
{
    computeAngles();

}


void Bone_Animation::setJacobian(int id)
{
    goal = target - blue_position_end;
    goalVector = { goal.x, goal.y, goal.z, 1.0f };
    std::cout << glm::length(goal) << std::endl;

    if (id == 0) {
        v1 = cross(glm::vec3(m_red_mat[0][2], m_red_mat[1][2], m_red_mat[2][2]), (blue_position_end - red_position_end));
        v2 = cross(glm::vec3(m_yellow_mat[0][2], m_yellow_mat[1][2], m_yellow_mat[2][2]), (blue_position_end - yellow_position_end));
        v3 = cross(glm::vec3(m_pink_mat[0][2], m_pink_mat[1][2], m_pink_mat[2][2]), (blue_position_end - pink_position_end));
    }

    if (id == 1) {
        v1 = cross(glm::vec3(m_red_mat[0][0], m_red_mat[1][0], m_red_mat[2][0]), (blue_position_end - red_position_end));
        v2 = cross(glm::vec3(m_yellow_mat[0][0], m_yellow_mat[1][0], m_yellow_mat[2][0]), (blue_position_end - yellow_position_end));
        v3 = cross(glm::vec3(m_pink_mat[0][0], m_pink_mat[1][0], m_pink_mat[2][0]), (blue_position_end - pink_position_end));
    }

    jacobian_matrix[id] = { { v1.x, v2.x, v3.x,0.f },
                           { v1.y, v2.y, v3.y,0.f } ,
                           { v1.z, v2.z, v3.z,0.f } ,
                           { 0.f, 0.f, 0.f ,1.f } };

}


void Bone_Animation::computeAngles()
{
    if (!stop) {
        setJacobian(0);
        anglesVector = get_transpose(0) * (goalVector); 


        angles[0] += anglesVector[0];
        angles[1] += anglesVector[1];
        angles[2] += anglesVector[2];



        angles[0] = fmod(angles[0], 360);
        angles[1] = fmod(angles[1], 360);
        angles[2] = fmod(angles[2], 360);


        forward_kinematics(yellowXangle, yellowYangle, angles[0], pinkXangle, pinkYangle, angles[1]
            , blueXangle, blueYangle, angles[2]);

        computeAngles2();

    }
}


void Bone_Animation::computeAngles2()
{
    if (!stop) {
        setJacobian(1);
        anglesVector = get_transpose(1) * (goalVector);

        angles[3] += anglesVector[0];
        angles[4] += anglesVector[1];
        angles[5] += anglesVector[2];


        angles[3] = fmod(angles[3], 360);
        angles[4] = fmod(angles[4], 360);
        angles[5] = fmod(angles[5], 360);



        forward_kinematics(angles[3], yellowYangle, yellowZangle, angles[4], pinkYangle, pinkZangle
            , angles[5], blueZangle, blueZangle);
    }
}


void Bone_Animation::shouldStop()
{
    if ((distance(blue_position_end.x, target.x, blue_position_end.y, target.y, blue_position_end.z, target.z)) <= errorFactor) {
        stop = true;
    }
}

glm::mat4 Bone_Animation::get_transpose(int id)
{
    glm::mat4 aux = glm::mat4(1.0) * alpha;

    return aux * glm::transpose(jacobian_matrix[id]);
}


float Bone_Animation::distance(float x1, float y1,
    float z1, float x2,
    float y2, float z2)
{
    float d = sqrt(pow(x2 - x1, 2) +
        pow(y2 - y1, 2) +
        pow(z2 - z1, 2) * 1.0);
    return d;
}

Result I am getting is: Blue cube keeps rotating around X-axis continuously and the other cubes dont rotate at all. Is there anything wrong in the way I implemented the algorithm. Please help! Thanks!

$\endgroup$
  • $\begingroup$ Please provide more information as to what you are wanting to achieve with this three joints robot. Specify if the joints are revolute or prismatic. $\endgroup$ – JJerome Nov 14 '19 at 22:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.