Originally Posted by
dainiusb
Any way to get yaw pitch and roll of a joint?
EDIT:
So I was bored today and tried to make uke to avoid dq..sort of..with a script..The main problem is that it lowers shoulder for no reason sometimes. Why is this happening? Also these calculations doesn't work for this type of thing. Thats why I need to know yaw pitch and roll of a joint.
First off, I'm terrible with rotation. And even worse with how TB uses quaternions, rotation matrices
and angle axes. But I took a look anyway.
I didn't find that the script ever lowered uke shoulders, but I noticed that the elbows aren't being extended. So I changed that, along with a couple things. After trying the script out, it seems somewhat able to prevent the upper body from dq'ing.
LUA code:
local function turn()
neck_pos = get_joint_pos2(1,0)
abs_pos = get_joint_pos2(1,3)
dX = neck_pos.x - abs_pos.x --delta x
dY = neck_pos.y - abs_pos.y --delta y
dZ = neck_pos.z - abs_pos.z --delta z
yaw = math.atan2(dZ, dX) --yaw calculation
pitch = math.atan2(math.sqrt(dZ * dZ + dX * dX), dY) + math.pi --pitch calculation
echo("yaw ="..yaw)
echo("pitch ="..pitch)
echo("-------------------------------------------")
if (pitch < 4.5) then -- Easier to react
--I thought contracting one pec and extending the other gives better coverage.
set_joint_state(1,4,2) --contract right pec
set_joint_state(1,7,1) --extend left pec
elseif(pitch > 5) then -- Easier to react
--I thought contracting one pec and extending the other gives better coverage.
set_joint_state(1,4,1) --extend right pec
set_joint_state(1,7,2) --contract left pec
end
if (yaw < 1.5) then
set_joint_state(1,8,2) --raise left shoulder (Seems safer to raise both shoulders to cover both flanks)
set_joint_state(1,6, 1) -- extend right elbow (Helps keeps the floor way)
set_joint_state(1,5,2) --raise right shoulder
set_joint_state(1,12,2) --extend right glute
elseif (yaw > 2.5) then
set_joint_state(1,5,2) --raise right shoulder (Seems safer to raise both shoulders to cover both flanks)
set_joint_state(1,9, 1) --- extend left elbow (Helps keeps the floor way)
set_joint_state(1,8,2) --raise left shoulder
set_joint_state(1,13,2) --extend left glute
end
end
turn()
add_hook("enter_freeze","turn",turn)