|
![](/i/fill.gif) |
Mark -
Here's a try at "repairing" the code. I think this method will scale to
any number of joints, as long as you don't go crazy!
I think the only difference is that I am figuring out how much each
joint should move by adding up two vector functions and weighting them
by a factor ("contraction"). This seems like a pretty general method.
I would like to add in "stiffness" to the joints that responds to the
MinAng and MaxAng parameters.
Anyway, it still needs to be cleaned up a bit but I hope it works for
you.
- Ben
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#debug "\n\nStarting IK Finger...\n\n"
//Some containers for numbers
#declare Length = array[3]{2,2,2}
#declare Rotate = array[3]{0,0,0}
#declare MinAng = array[3]{-20,-90,-2}
#declare MaxAng = array[3]{90,90,90}
//Try both of these joint definitions!!!
#declare Joint = array[3]{<1,0,0>, <0,1,0>, <0,0,1>}
//#declare Joint = array[3]{<0,0,1>, <0,0,1>, <0,0,1>}
#declare tJoint = array[3]{<0,0,1>, <0,1,0>, <0,0,1>}
#declare Base = array[3]{<0,0,0>, <0,0,0>, <0,0,0>}
#declare Tip = array[3]{<0,0,0>, <0,0,0>, <0,0,0>}
#declare Torque = array[3]{0,0,0}
#declare seed1 = seed(-1);
//
// Important Constants
//
#declare TargetLocation = <2,3,0>;
#declare FingerLocation = <0,0,0>;
#declare Delta = 1.0;
#declare TargetBailout=0.3;
#declare TorqueBailout=0.001;
#declare MaxIterations=800;
#declare TargetReached=no;
#declare contraction = 1/1;
//
// Define the workhorse functions to
// calculate angular and radial "distances"
//
#macro DistAng(VA,VB)
( 1.0 - vdot(VA,VB)/vlength(VA)/vlength(VB) )/2.0
#end
#macro DistRad(V1,V2)
( vlength(V1) - vlength(V2) )
#end
#macro PlaneNormal(V1,V2)
vnormalize( vcross(V1,V2) )
#end
#macro WeightAng(Target,Tip,Joint)
DistAng(Target,Tip)*vdot(PlaneNormal(Tip,Target),Joint)
#end
//random numbers added so the program wouldn't freeze up
//if the joints were in a straight line
#macro WeightRad(Target,Tip,Leg,Joint)
(
DistRad(Target,Tip)*vdot(PlaneNormal(Leg,Tip-Leg),Joint)/vlength(Target)
+ (rand(seed1)-0.5)*1e-5 )
#end
//
//Some tools
//
#macro ShowVector(V1, V2)
cone{V1, 0.05, V2, 0.0 texture{pigment{color rgb<1,1,0>}}}
#end
#macro Vector2Text(V)
vstr(3,V,", ",4,2)
#end
#macro Number2Text(N)
str(N,7,4)
#end
//
// Define Common Objects
//
#macro MakeJoint(V1,V2)
merge{
sphere{V1, 0.2}
cone{V1, 0.2, V2, 0.05}
sphere{V2, 0.05}
cylinder{V1, V2 + 50*(V2-V1), 0.01}
}
#end
#declare Target=
union{
cone{0.06*x, 0, 1*x, 0.2}
cone{0.06*x, 0, 1*x, 0.2 rotate 90*z}
cone{0.06*x, 0, 1*x, 0.2 rotate 180*z}
cone{0.06*x, 0, 1*x, 0.2 rotate 270*z}
sphere{0, TargetBailout}
texture{pigment{color rgbt<0,1,1,0.5>}}
}
#if (clock_on)
#declare current_frame=(clock/clock_delta)+1;
#else
#declare current_frame=1;
#end
#debug concat("\n ******* ", Number2Text(current_frame), " *******\n")
//Calculates positions of each tip, and if the last one is close to the
target,
// sets a variable to say so.
#macro CalcTipsFK()
#declare Base[0] = FingerLocation;
#declare tJoint[0] = Joint[0];
#declare Tip[0] = Base[0] +
vaxis_rotate(Length[0]*x,tJoint[0],Rotate[0]);
#declare Base[1] = Tip[0];
#declare tJoint[1] = vaxis_rotate(Joint[1]
,tJoint[0],Rotate[0]);
#declare Tip[1] =
vaxis_rotate(Length[1]*x,tJoint[0],Rotate[0]);
#declare Tip[1] = Base[1] + vaxis_rotate(Tip[1]
,tJoint[1],Rotate[1]);
#declare Base[2] = Tip[1];
#declare tJoint[2] = vaxis_rotate( Joint[2]
,tJoint[0],Rotate[0]);
#declare tJoint[2] = vaxis_rotate(tJoint[2]
,tJoint[1],Rotate[1]);
#declare Tip[2] =
vaxis_rotate(Length[2]*x,tJoint[0],Rotate[0]);
#declare Tip[2] = vaxis_rotate(Tip[2]
,tJoint[1],Rotate[1]);
#declare Tip[2] = Base[2] + vaxis_rotate(Tip[2]
,tJoint[2],Rotate[2]);
#if (vlength(TargetLocation-Tip[2])<TargetBailout)
#declare TargetReached=yes;
#debug "\n**Target Reached"
#debug concat("\n Iterations: ", Number2Text(ct))
#else
#declare TargetReached=no;
#end
#end
//rotate the joint to point at the target
#macro ReorientTip2()
CalcTipsFK()
#declare TotLength = Length[0]+Length[1]+Length[2];
#declare ct = 0;
#declare finished = false;
#while (!finished & ct<MaxIterations)
#if (TargetReached)
#declare finished = true;
#debug "TR"
#else
CalcTipsFK()
#declare TotTip = Tip[2]-Base[0];
#declare tTarget = TargetLocation-Base[0];
#declare tTip = Tip[2]-Base[0];
#declare tLegLast = x;
#declare tLeg = Tip[0]-Base[0];
#declare tLength = Length[0]+Length[1]+Length[2];
#declare Torque[0] = ( WeightAng(tTarget,tTip,Joint[0]) +
0*WeightRad(tTarget,TotTip,tLeg,Joint[0])*contraction );
#declare tTarget = TargetLocation-Base[1];
#declare tTip = Tip[2]-Base[1];
#declare tLegLast = tLeg;
#declare tLeg = Tip[1]-Base[1];
#declare tLength = Length[1]+Length[2];
#declare Torque[1] = ( 0*WeightAng(tTarget,tTip,Joint[1]) +
WeightRad(tTarget,TotTip,tLeg,Joint[1])*contraction );
#declare tTarget =
TargetLocation-Base[2];
#declare tTip = Tip[2]-Base[2];
#declare tLegLast = tLeg;
#declare tLeg = Tip[2]-Base[2];
#declare tLength = Length[2];
#declare Torque[2] = ( WeightAng(tTarget,tTip,Joint[2]) +
0*WeightRad(tTarget,TotTip,tLeg,Joint[2])*contraction );
#declare Weight = abs(Torque[0]) + abs(Torque[1]) + abs(Torque[2])
+ 1e-5;
//Put in the small constant so there wouldn't be any "divide by
zero" errors
#declare Rotate[0] = max(Rotate[0] + Delta * Torque[0] / Weight,
MinAng[0]);
#declare Rotate[1] = max(Rotate[1] + Delta * Torque[1] / Weight,
MinAng[1]);
#declare Rotate[2] = max(Rotate[2] + Delta * Torque[2] / Weight,
MinAng[2]);
#declare Rotate[0] = min( Rotate[0], MaxAng[0] );
#declare Rotate[1] = min( Rotate[1], MaxAng[1] );
#declare Rotate[2] = min( Rotate[2], MaxAng[2] );
#declare ct = ct + 1;
#debug concat("\n Rotate[0]: ", Number2Text(Rotate[0]))
#debug concat(" Rotate[1]: ", Number2Text(Rotate[1]))
#debug concat(" Rotate[2]: ", Number2Text(Rotate[2]))
//#debug concat("\n Iterations: ", Number2Text(ct))
//#debug concat("\n Torque[2]: ", Number2Text(Torque[2]/Delta))
#end
#end
#end
ReorientTip2()
CalcTipsFK()
//
// Somehow, these declarations needed to be moved to work...
//
#declare Joint0 =
object{
MakeJoint(FingerLocation,Tip[0])
texture{pigment{color rgbt<1,0,0,0.5>}}
}
#declare Joint1 =
object{
MakeJoint(Tip[0],Tip[1])
texture{pigment{color rgbt<0,1,0,0.5>}}
}
#declare Joint2 = object{
MakeJoint(Tip[1],Tip[2])
texture{pigment{color rgbt<0,0,1,0.5>}}
}
object{Target translate TargetLocation}
object{Joint0}
object{Joint1}
object{Joint2}
//
//My stupid version of your "robot-testrig.inc" file
//
background{ rgb <1,1,1> }
#declare Camera_Location=<3/2, 3/2, -20>;
#declare Camera_Target=<3/2,3/2,0>;
#declare Camera_Angle=25;
#declare Use_AreaLights=no;
#declare Use_Rad=no;
#declare Ground_Height=-1;
#declare Draw_Sky=no;
#declare Draw_Ground=no;
// create a regular point light source
light_source
{
0*x // light's position (translated below)
color red 1.0 green 1.0 blue 1.0 // light's color
translate <-20, 40, -20>
}
camera {
location Camera_Location
look_at Camera_Target
angle Camera_Angle
}
//#include "robot-testrig.inc"
#debug "\n\nEnd\n\n"
Post a reply to this message
|
![](/i/fill.gif) |