|
![](/i/fill.gif) |
Thank you!!
I'm at work at the moment, but I'll have a look later...
thanks again,
Mark Hanford
http://www.mrhanford.com/povray
"Ben Birdsey" <cla### [at] mail com> wrote in message
news:3C70B93C.E745C420@mail.com...
>
> 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) |