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";
}
