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 agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.