public TaskspaceHandControlState(
String namePrefix,
HandControlState stateEnum,
MomentumBasedController momentumBasedController,
int jacobianId,
RigidBody base,
RigidBody endEffector,
YoVariableRegistry parentRegistry) {
super(stateEnum);
name =
namePrefix
+ FormattingTools.underscoredToCamelCase(this.getStateEnum().toString(), true)
+ "State";
registry = new YoVariableRegistry(name);
taskspaceConstraintData.set(base, endEffector);
this.momentumBasedController = momentumBasedController;
this.jacobianId = jacobianId;
this.base = base;
this.endEffector = endEffector;
parentRegistry.addChild(registry);
}
protected void submitDesiredAcceleration(SpatialAccelerationVector handAcceleration) {
taskspaceConstraintData.set(handAcceleration);
momentumBasedController.setDesiredSpatialAcceleration(jacobianId, taskspaceConstraintData);
}
protected void setEndEffector(RigidBody newEndEffector) {
this.endEffector = newEndEffector;
taskspaceConstraintData.set(base, endEffector);
}
protected void setBase(RigidBody newBase) {
this.base = newBase;
taskspaceConstraintData.set(base, endEffector);
}