AvoidanceHandlerAbstract(iKinLimb &limb) : chain(*limb.asChain()) { iKinLimb *limb_; Matrix HN=eye(4,4); limb_=new iKinLimb(limb); iKinChain *c1=limb_->asChain(); c1->rmLink(9); c1->rmLink(8); c1->rmLink(7); //TODO for forearm skin, the link 7 should probably remain there; but then the HN will have to be adapted appropriately to set the translation from the new frame (wrist instead of elbow it seems) HN(2,3)=0.1373; c1->setHN(HN); limb_=new iKinLimb(limb); iKinChain *c2=limb_->asChain(); c2->rmLink(9); c2->rmLink(8); c2->rmLink(7); //TODO for forearm skin, the link 7 should probably remain there; but then the HN will have to be adapted appropriately to set the translation from the new frame (wrist instead of elbow it seems) HN(2,3)=0.1373/2.0; c2->setHN(HN); // the first item is the original chain // used to account for the end-effector chainCtrlPoints.push_back(&chain); chainCtrlPoints.push_back(c1); chainCtrlPoints.push_back(c2); type="none"; }
AvoidanceHandlerAbstract(iKinLimb &limb) : chain(*limb.asChain()) { iKinLimb *limb_; Matrix HN=eye(4,4); limb_=new iKinLimb(limb); iKinChain *c1=limb_->asChain(); c1->rmLink(9); c1->rmLink(8); c1->rmLink(7); HN(2,3)=0.1373; c1->setHN(HN); limb_=new iKinLimb(limb); iKinChain *c2=limb_->asChain(); c2->rmLink(9); c2->rmLink(8); c2->rmLink(7); HN(2,3)=0.1373/2.0; c2->setHN(HN); // the first item is the original chain // used to account for the end-effector chainCtrlPoints.push_back(&chain); chainCtrlPoints.push_back(c1); chainCtrlPoints.push_back(c2); type="none"; }