Struggeling with Jacobian Inverse Kinematics
up vote
0
down vote
favorite
I am really struggling to find the right Jacobian for my 4DOF robot arm in 3D-space. First joint rotates around the y-axis, second around z-axis, third around z-axis and last one around the y-axis. All links are 1 unit long. My problem is probably the orientation vector zi
because I know that my Forward Kinematics are correct. j1_pos
-j4_pos
are the positions of the joints. ee_pos
is the end-effector. All positions are correct. My thought was just to multiply the respective axis vectors with the transformation matrices to get the orientation vectors zi
.
Any advice would be much appreciated.
I am using the formula
J = [Jpi] = [zi x (pe - pi)]
[Joi] [ zi ]
def Jacobian(self,joint_angles):
jacobian = np.zeros((6,4))
j1_trans = self.link_transform_y(joint_angles[0])
j2_trans = self.link_transform_z(joint_angles[1])
j3_trans = self.link_transform_z(joint_angles[2])
j4_trans = self.link_transform_y(joint_angles[3])
ee_pos = (j1_trans*j2_trans*j3_trans*j4_trans)[0:3, 3]
j4_pos = (j1_trans*j2_trans*j3_trans)[0:3, 3]
j3_pos = (j1_trans*j2_trans)[0:3, 3]
j2_pos = (j1_trans)[0:3, 3]
j1_pos = np.zeros((3,1))
pos3D = np.zeros(3)
pos3D = (ee_pos-j1_pos).T
z0_vector = [0, 1, 0]
jacobian[0:3, 0] = np.cross(z0_vector, pos3D)
pos3D[0:3] = (ee_pos-j2_pos).T
z1_vector = (j1_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 1] = np.cross(z1_vector, pos3D)
pos3D[0:3] = (ee_pos-j3_pos).T
z2_vector = (j1_trans*j2_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 2] = np.cross(z2_vector, pos3D)
pos3D[0:3] = (ee_pos-j4_pos).T
z3_vector = (j1_trans*j2_trans*j3_trans*np.array([0, 1, 0, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 3] = np.cross(z3_vector, pos3D)
jacobian[3:6, 0] = z0_vector
jacobian[3:6, 1] = z1_vector
jacobian[3:6, 2] = z2_vector
jacobian[3:6, 3] = z3_vector
return jacobian
python robotics inverse-kinematics
add a comment |
up vote
0
down vote
favorite
I am really struggling to find the right Jacobian for my 4DOF robot arm in 3D-space. First joint rotates around the y-axis, second around z-axis, third around z-axis and last one around the y-axis. All links are 1 unit long. My problem is probably the orientation vector zi
because I know that my Forward Kinematics are correct. j1_pos
-j4_pos
are the positions of the joints. ee_pos
is the end-effector. All positions are correct. My thought was just to multiply the respective axis vectors with the transformation matrices to get the orientation vectors zi
.
Any advice would be much appreciated.
I am using the formula
J = [Jpi] = [zi x (pe - pi)]
[Joi] [ zi ]
def Jacobian(self,joint_angles):
jacobian = np.zeros((6,4))
j1_trans = self.link_transform_y(joint_angles[0])
j2_trans = self.link_transform_z(joint_angles[1])
j3_trans = self.link_transform_z(joint_angles[2])
j4_trans = self.link_transform_y(joint_angles[3])
ee_pos = (j1_trans*j2_trans*j3_trans*j4_trans)[0:3, 3]
j4_pos = (j1_trans*j2_trans*j3_trans)[0:3, 3]
j3_pos = (j1_trans*j2_trans)[0:3, 3]
j2_pos = (j1_trans)[0:3, 3]
j1_pos = np.zeros((3,1))
pos3D = np.zeros(3)
pos3D = (ee_pos-j1_pos).T
z0_vector = [0, 1, 0]
jacobian[0:3, 0] = np.cross(z0_vector, pos3D)
pos3D[0:3] = (ee_pos-j2_pos).T
z1_vector = (j1_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 1] = np.cross(z1_vector, pos3D)
pos3D[0:3] = (ee_pos-j3_pos).T
z2_vector = (j1_trans*j2_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 2] = np.cross(z2_vector, pos3D)
pos3D[0:3] = (ee_pos-j4_pos).T
z3_vector = (j1_trans*j2_trans*j3_trans*np.array([0, 1, 0, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 3] = np.cross(z3_vector, pos3D)
jacobian[3:6, 0] = z0_vector
jacobian[3:6, 1] = z1_vector
jacobian[3:6, 2] = z2_vector
jacobian[3:6, 3] = z3_vector
return jacobian
python robotics inverse-kinematics
add a comment |
up vote
0
down vote
favorite
up vote
0
down vote
favorite
I am really struggling to find the right Jacobian for my 4DOF robot arm in 3D-space. First joint rotates around the y-axis, second around z-axis, third around z-axis and last one around the y-axis. All links are 1 unit long. My problem is probably the orientation vector zi
because I know that my Forward Kinematics are correct. j1_pos
-j4_pos
are the positions of the joints. ee_pos
is the end-effector. All positions are correct. My thought was just to multiply the respective axis vectors with the transformation matrices to get the orientation vectors zi
.
Any advice would be much appreciated.
I am using the formula
J = [Jpi] = [zi x (pe - pi)]
[Joi] [ zi ]
def Jacobian(self,joint_angles):
jacobian = np.zeros((6,4))
j1_trans = self.link_transform_y(joint_angles[0])
j2_trans = self.link_transform_z(joint_angles[1])
j3_trans = self.link_transform_z(joint_angles[2])
j4_trans = self.link_transform_y(joint_angles[3])
ee_pos = (j1_trans*j2_trans*j3_trans*j4_trans)[0:3, 3]
j4_pos = (j1_trans*j2_trans*j3_trans)[0:3, 3]
j3_pos = (j1_trans*j2_trans)[0:3, 3]
j2_pos = (j1_trans)[0:3, 3]
j1_pos = np.zeros((3,1))
pos3D = np.zeros(3)
pos3D = (ee_pos-j1_pos).T
z0_vector = [0, 1, 0]
jacobian[0:3, 0] = np.cross(z0_vector, pos3D)
pos3D[0:3] = (ee_pos-j2_pos).T
z1_vector = (j1_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 1] = np.cross(z1_vector, pos3D)
pos3D[0:3] = (ee_pos-j3_pos).T
z2_vector = (j1_trans*j2_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 2] = np.cross(z2_vector, pos3D)
pos3D[0:3] = (ee_pos-j4_pos).T
z3_vector = (j1_trans*j2_trans*j3_trans*np.array([0, 1, 0, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 3] = np.cross(z3_vector, pos3D)
jacobian[3:6, 0] = z0_vector
jacobian[3:6, 1] = z1_vector
jacobian[3:6, 2] = z2_vector
jacobian[3:6, 3] = z3_vector
return jacobian
python robotics inverse-kinematics
I am really struggling to find the right Jacobian for my 4DOF robot arm in 3D-space. First joint rotates around the y-axis, second around z-axis, third around z-axis and last one around the y-axis. All links are 1 unit long. My problem is probably the orientation vector zi
because I know that my Forward Kinematics are correct. j1_pos
-j4_pos
are the positions of the joints. ee_pos
is the end-effector. All positions are correct. My thought was just to multiply the respective axis vectors with the transformation matrices to get the orientation vectors zi
.
Any advice would be much appreciated.
I am using the formula
J = [Jpi] = [zi x (pe - pi)]
[Joi] [ zi ]
def Jacobian(self,joint_angles):
jacobian = np.zeros((6,4))
j1_trans = self.link_transform_y(joint_angles[0])
j2_trans = self.link_transform_z(joint_angles[1])
j3_trans = self.link_transform_z(joint_angles[2])
j4_trans = self.link_transform_y(joint_angles[3])
ee_pos = (j1_trans*j2_trans*j3_trans*j4_trans)[0:3, 3]
j4_pos = (j1_trans*j2_trans*j3_trans)[0:3, 3]
j3_pos = (j1_trans*j2_trans)[0:3, 3]
j2_pos = (j1_trans)[0:3, 3]
j1_pos = np.zeros((3,1))
pos3D = np.zeros(3)
pos3D = (ee_pos-j1_pos).T
z0_vector = [0, 1, 0]
jacobian[0:3, 0] = np.cross(z0_vector, pos3D)
pos3D[0:3] = (ee_pos-j2_pos).T
z1_vector = (j1_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 1] = np.cross(z1_vector, pos3D)
pos3D[0:3] = (ee_pos-j3_pos).T
z2_vector = (j1_trans*j2_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 2] = np.cross(z2_vector, pos3D)
pos3D[0:3] = (ee_pos-j4_pos).T
z3_vector = (j1_trans*j2_trans*j3_trans*np.array([0, 1, 0, 0]).reshape(4,1))[0:3].T
jacobian[0:3, 3] = np.cross(z3_vector, pos3D)
jacobian[3:6, 0] = z0_vector
jacobian[3:6, 1] = z1_vector
jacobian[3:6, 2] = z2_vector
jacobian[3:6, 3] = z3_vector
return jacobian
python robotics inverse-kinematics
python robotics inverse-kinematics
edited Nov 11 at 1:13
Thomas Fritsch
4,595121833
4,595121833
asked Nov 10 at 15:19
Frederik Kelbel
13
13
add a comment |
add a comment |
active
oldest
votes
active
oldest
votes
active
oldest
votes
active
oldest
votes
active
oldest
votes
Sign up or log in
StackExchange.ready(function ()
StackExchange.helpers.onClickDraftSave('#login-link');
);
Sign up using Google
Sign up using Facebook
Sign up using Email and Password
Post as a guest
Required, but never shown
StackExchange.ready(
function ()
StackExchange.openid.initPostLogin('.new-post-login', 'https%3a%2f%2fstackoverflow.com%2fquestions%2f53240351%2fstruggeling-with-jacobian-inverse-kinematics%23new-answer', 'question_page');
);
Post as a guest
Required, but never shown
Sign up or log in
StackExchange.ready(function ()
StackExchange.helpers.onClickDraftSave('#login-link');
);
Sign up using Google
Sign up using Facebook
Sign up using Email and Password
Post as a guest
Required, but never shown
Sign up or log in
StackExchange.ready(function ()
StackExchange.helpers.onClickDraftSave('#login-link');
);
Sign up using Google
Sign up using Facebook
Sign up using Email and Password
Post as a guest
Required, but never shown
Sign up or log in
StackExchange.ready(function ()
StackExchange.helpers.onClickDraftSave('#login-link');
);
Sign up using Google
Sign up using Facebook
Sign up using Email and Password
Sign up using Google
Sign up using Facebook
Sign up using Email and Password
Post as a guest
Required, but never shown
Required, but never shown
Required, but never shown
Required, but never shown
Required, but never shown
Required, but never shown
Required, but never shown
Required, but never shown
Required, but never shown