スポンサーサイト

-------- | --:--

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。

逆運動学

2007-12-25 | 21:13

赤い円をドラッグで動かすと、二つの線がロボットのアームのように動きます。

このようにリンク機構の目標の座標が与えられている場合に、関節の角度を求める事を逆運動学という。
この例のように平面上で2自由度の場合であれば角度を求めるのは非常に簡単。
package
{
        import flash.display.*;
        import flash.geom.*;
        import flash.events.*;
        import flash.filters.*;
        import flash.utils.*;
        
        import caurina.transitions.Tweener;
        
        [SWF(width="400", height="400", framerate="30", backgroundColor="#fff0f0")]
        public class Link extends Sprite
        {
                private var arm1:Arm;
                private var arm2:Arm;
                private var point:Point;
                private var ma:Matrix;
                public function Link()
                {
                        
                        addChild(arm1 = new Arm( 300, 0xff));
                        arm1.x = 0;
                        arm1.y = 0;
                        addChild(arm2 = new Arm( 300, 0xff00));
                        arm2.x = 300;
                        arm2.y = 250;
                        
                        var red:Sprite = createCircle( 0xFF0000, 10 );
                        addChild(red);
                        red.x = 300;
                        red.y = 300;
                        red.addEventListener( MouseEvent.MOUSE_DOWN, pickup );
                        red.addEventListener( MouseEvent.MOUSE_UP, place );
                        moveArm(red.x,red.y);
                }
                
                public function createCircle( color:uint, radius:Number ):Sprite {
                        var shape:Sprite = new Sprite( );
                        shape.graphics.beginFill( color );
                        shape.graphics.drawCircle( 0, 0, radius );
                        shape.graphics.endFill( );
                        return shape;
                }
                
                public function pickup( event:MouseEvent ):void {
                        // ドラッグ処理を開始して、影フィルターをつける
                        event.target.startDrag( );
                        event.target.filters = [ new DropShadowFilter( ) ];
                }
                
                public function place( event:MouseEvent ):void {
                        // ドラッグ処理を終了して影フィルターをオフ
                        event.target.stopDrag( );
                        event.target.filters = null;
                        
                        moveArm(event.target.x, event.target.y);
                }
                
                private function moveArm(x:int, y:int):void{
                        var t2:Number = Math.acos((x*x + y*y - arm1.len*arm1.len - arm2.len*arm2.len)
                        /(2*arm1.len*arm2.len));
                        var t1:Number = Math.atan(y/x) -
                        Math.atan((arm2.len * Math.sin(t2)) /
                        (arm1.len + arm2.len * Math.cos(t2)));
                        
                        Tweener.addTween(arm1, {rotation:t1*180/Math.PI,
                                                 onUpdate:function():void{
                                        arm2.x = arm1.len*Math.cos(arm1.rotation /180*Math.PI);
                                        arm2.y = arm1.len*Math.sin(arm1.rotation/180*Math.PI);
                                },
                                onComplete:function():void{
                                        
                                        Tweener.addTween (arm2, { rotation:(t1+t2)*180/Math.PI, time:2, transition:"linear"}); },
                                                                    time:2, transition:"linear"});
                }             
        }
}

import flash.display.*;
import flash.geom.*;
internal class Arm extends Sprite{
        public var len:int;
        public function Arm(len:int, c:int = 0xff){
                this.len = len;
                graphics.lineStyle(30, c, 0.8);
                graphics.moveTo(0 , 0);
                graphics.lineTo(len, 0);
        }
}

スポンサーサイト
上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。