I’m trying to build a simulation of a hexapod (CH3-R) using Autodesk Maya (because thats what i know). I’m trying to make this code work for any one leg. I tested it on a leg point from the origin along the X axis and it workd fine. But with i pass to it a different leg, i get problems. The coaxAngle variable always uses the global coordinate system and not local to the leg. How do I incorporate the leg offset angle (legs go 0deg, 60deg, 120deg…) into the code so that I can move each leg +180deg to -180deg relative to its original drawn position? Is this even how I should be doing this in the code? Maybe there is a better/neater way of writing the code for this operation?
[code]global proc float] hexapod( string $leg )
{
// end effector world space position (position of foot)
float $x2 = getAttr ($leg + "endEffector.tx");
float $y2 = getAttr ($leg + "endEffector.ty");
float $z2 = getAttr ($leg + "endEffector.tz");
// start effector world space position (position of coxa)
float $x1 = getAttr ($leg + "startEffector.tx");
float $y1 = getAttr ($leg + "startEffector.ty");
float $z1 = getAttr ($leg + "startEffector.tz");
// length of the joints
float $coxaLen = getAttr ($leg + "femur_joint.tx");
float $femurLen = getAttr ($leg + "tibia_joint.tx");
float $tibiaLen = getAttr ($leg + "tibiaTip_joint.tx");
// get the translation of the coxa joint
float $coxaTX = getAttr ($leg + "startEffector.tx");
float $coxaTY = getAttr ($leg + "startEffector.ty");
float $coxaTZ = getAttr ($leg + "startEffector.tz");
// calculating displacement from coxa to foot
$dx = $x2 - $x1;
$dy = $y2 - $y1;
$dz = $z2 - $z1;
// Pi…
$pi = 3.141592;
// $e: projection of displacement vector onto ground plane ($f vector)
// $f: displacement vector from coxa to foot (length from coxa to foot)
$e = sqrt(pow($dx,2) + pow($dz,2)) - $coxaLen;
$f = sqrt(pow($e,2) + pow($dy,2));
// calculate relative angles within the coxa/femur/tibia joint setup
// $u: angle between $f vector and ground plane
// $v: interior angle between femur and $f
// $w: interior angle between femur and tibia
$u = atand($dy/$e);
$v = acosd((pow($femurLen,2) - pow($tibiaLen,2) + pow($f,2)) / (2 * $femurLen * $f));
$w = acosd((pow($femurLen,2) + pow($tibiaLen,2) - pow($f,2)) / (2 * $femurLen * $tibiaLen));
// calculate final angles for movement of the feet in main program
$coxaAngle = (-1 * (rad_to_deg(hexapod.catchTanException($dz, $dx))));
$femurAngle = ($u + $v);
$tibiaAngle = 180 + $w;
// return angles and coxa translation
float $return];
$return = { $coxaAngle, $femurAngle, $tibiaAngle, $coxaTX, $coxaTY, $coxaTZ };
return $return;
}
// ----------------------------------
// prevent tangent function from erroring out
// ----------------------------------
global proc float hexapod.catchTanException( float $tanX, float $tanY )
{
float $pi = 3.141592;
float $tan;
if ($tanY == 0) {
if ($tanY >= 0) {
$tan = 0.0;
} else {
$tan = $pi;
}} else {
if ($tanY == 0) {
if ($tanX > 0) {
$tan = ($pi / 2.0);
} else {
$tan = ($pi / 2.0);
}} else {
if ($tanY > 0) {
$tan = atan($tanX / $tanY);
} else {
if ($tanX > 0) {
$tan = atan($tanX / $tanY) + $pi;
} else {
$tan = atan($tanX / $tanY) - $pi;
}}}}
// return the angle
return $tan;
}[/code]