public FourBarKinematicLoopJacobianSolver(PassiveRevoluteJoint outputJoint) { this.jointsForJacobianCalculation = getJointsForJacobianCalculation(); this.geometricJacobianToColumnJacobian = createGeometricJacobianToColumnJacobianMatrix(jointsForJacobianCalculation); this.geometricJacobianFrame = outputJoint.getFrameAfterJoint(); this.geometricJacobian = new GeometricJacobian(jointsForJacobianCalculation, geometricJacobianFrame); }
public FourBarKinematicLoopJacobianSolver(PassiveRevoluteJoint outputJoint) { this.jointsForJacobianCalculation = getJointsForJacobianCalculation(); this.geometricJacobianToColumnJacobian = createGeometricJacobianToColumnJacobianMatrix(jointsForJacobianCalculation); this.geometricJacobianFrame = outputJoint.getFrameAfterJoint(); this.geometricJacobian = new GeometricJacobian(jointsForJacobianCalculation, geometricJacobianFrame); }
expectedAngularY = qdMaster; expectedAngularZ = 0.0; outputJointFrame = passiveJointB.getFrameAfterJoint(); break; case 2: expectedAngularY = 0.0; expectedAngularZ = 0.0; outputJointFrame = passiveJointC.getFrameAfterJoint(); break; case 3: expectedAngularY = qdMaster; expectedAngularZ = 0.0; outputJointFrame = passiveJointD.getFrameAfterJoint(); break; default:
FramePoint3D jointBPosition = new FramePoint3D(passiveJointB.getFrameAfterJoint()); FramePoint3D jointCPosition = new FramePoint3D(passiveJointC.getFrameAfterJoint()); FramePoint3D jointDZeroAnglePosition = new FramePoint3D(passiveJointD.getFrameAfterJoint()); FramePoint3D jointDClosedLoopPosition = new FramePoint3D(masterJointA.getFrameBeforeJoint(), jointDInJointABeforeFrame);
FramePoint jointBPosition = new FramePoint(passiveJointB.getFrameAfterJoint()); FramePoint jointCPosition = new FramePoint(passiveJointC.getFrameAfterJoint()); FramePoint jointDZeroAnglePosition = new FramePoint(passiveJointD.getFrameAfterJoint()); FramePoint jointDClosedLoopPosition = new FramePoint(masterJointA.getFrameBeforeJoint(), jointDInJointABeforeFrame);
passiveJointD.setQ(0.0); masterJointA.getFrameAfterJoint().update(); passiveJointB.getFrameAfterJoint().update(); passiveJointC.getFrameAfterJoint().update(); passiveJointD.getFrameAfterJoint().update();