0
$\begingroup$

I have working inverse kinematic code in python for 3dof manipulator robotic arm. I am trying to model that robotic arm with bone in a blender. I have already written the python code to do it in the blender. As an object, I am using Cube to track it by bone.

I am giving coordinate of Cube as the input of my inverse kinematics algorithm and then generate angles of each bone, but I am not understanding why my bone does not go to coordinate of Cube.

can anyone please help me find the problem. I think maybe the problem would be in the coordinate system transformation. but I do not know how to solve it.

    # length of each joint
joint_len_1 = 33. # mm 3.3cm
joint_len_2 = 105. # mm 10.5cm
joint_len_3 = 98. # mm 9.8cm

# theta_1 = np.arctan(Y, X) # eq1
theta_1 = math.atan(Y / X)  # eq1
r1 = np.sqrt(X ** 2 + Y ** 2)  # eq2
r2 = Z - joint_len_1  # eq3
phi_2 = math.atan(r2 / r1)  # eq4
r3 = np.sqrt(r1 ** 2 + r2 ** 2)  # eq5

phi_1 = np.arccos((joint_len_3 ** 2 - joint_len_2 ** 2 - r3 ** 2) / (-2.0 * joint_len_2 * r3))  # eq6

theta_2 = phi_2 - phi_1  # eq7

phi_3 = np.arccos(((r3 ** 2) - (joint_len_2 ** 2) - (joint_len_3 ** 2)) / (-2.0 * joint_len_2 * joint_len_3)) # eq8
theta_3 = np.pi - phi_3  # eq9

python code in blender.

angleX = math.radians(0.)
angleY = math.radians(0.)
angleZ = math.radians(0.)

ob = bpy.data.objects['Armature']
cube = bpy.data.objects['Cube.003']

v = cube.data.vertices[0].co
mat = cube.matrix_world

# Multiply matrix by vertex
col = mat * cube.location
x, y, z = col#[3][:3]
print('world cordinates')
print(x, y, z)


bpy.context.scene.objects.active = ob
bpy.ops.object.mode_set(mode='POSE')
sholder_angle, upper_arm_angle, forearm_angle = ik_3dof.ik_3dof(x, y, z)
#sholder_angle, upper_arm_angle, forearm_angle = ik_3dof.ik_3dof(cube.location.x, cube.location.y, cube.location.z)

sholder = ob.pose.bones['sholder']     
upper_arm = ob.pose.bones['upper_arm']
forearm = ob.pose.bones['forearm']

sholder.rotation_mode = 'XYZ'
upper_arm.rotation_mode = 'XYZ'
forearm.rotation_mode = 'XYZ'

sholder.rotation_euler =(sholder_angle, angleY , angleZ)

upper_arm.rotation_euler =(upper_arm_angle , math.radians(180.), angleZ)
forearm.rotation_euler =(forearm_angle , angleY, angleZ)

bpy.ops.object.mode_set(mode='OBJECT')

enter image description here

$\endgroup$

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Browse other questions tagged or ask your own question.