Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/impedance control #533

Open
wants to merge 7 commits into
base: develop
Choose a base branch
from
Open

Feature/impedance control #533

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
8f26714
extracted from Coen's work, no reference spreading stuff
LuigiPenco93 Dec 12, 2024
19e405b
messages for impedance
LuigiPenco93 Dec 12, 2024
ad5c4bd
fixed imports
LuigiPenco93 Dec 12, 2024
c47ada6
Merge branch 'develop' into feature/impedance-control
LuigiPenco93 Dec 12, 2024
8a91523
added flag for impedance in message hybrid traj
LuigiPenco93 Dec 12, 2024
1902b81
added impedance option in behavior action
LuigiPenco93 Dec 13, 2024
120c10f
Merge branch 'develop' into feature/impedance-control
LuigiPenco93 Dec 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions .../java/us/ihmc/commonWalkingControlModules/configurations/WalkingControllerParameters.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
import us.ihmc.commonWalkingControlModules.momentumBasedController.optimization.OneDoFJointPrivilegedConfigurationParameters;
import us.ihmc.euclid.geometry.Pose3D;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
import us.ihmc.robotics.controllers.pidGains.PID3DGainsReadOnly;
import us.ihmc.robotics.controllers.pidGains.PIDGainsReadOnly;
import us.ihmc.robotics.controllers.pidGains.implementations.PDGains;
import us.ihmc.robotics.controllers.pidGains.implementations.PID3DConfiguration;
Expand Down Expand Up @@ -236,6 +237,16 @@ public List<GroupParameter<PID3DConfiguration>> getTaskspacePositionControlGains
return new ArrayList<>();
}

public PID3DGainsReadOnly getImpedanceHandPositionControlGains()
{
return null;
}

public PID3DGainsReadOnly getImpedanceHandOrientationControlGains()
{
return null;
}

/**
* Returns a map with default control modes for each rigid body.
* <p>
Expand Down Expand Up @@ -272,6 +283,15 @@ public boolean enableFunctionGeneratorMode(String rigidBodyName)
return false;
}

/**
* If true, the rigid body spatial control state for the given rigid body will be setup
* with impedance control
*/
public boolean enableImpedanceControl(String rigidBodyName)
{
return false;
}

/**
* The map returned contains the default rigid body poses in their respective base frame. For
* example, if the base frame of the chest body is the pelvis z-up frame this should contain the
Expand Down
2 changes: 1 addition & 1 deletion ...a/us/ihmc/commonWalkingControlModules/controlModules/pelvis/PelvisHeightControlState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ public PelvisHeightControlState(HighLevelHumanoidControllerToolbox controllerToo
YoDouble yoTime = controllerToolbox.getYoTime();
YoGraphicsListRegistry graphicsListRegistry = controllerToolbox.getYoGraphicsListRegistry();

positionController = new RigidBodyPositionController(pelvis, elevator, elevator, pelvisFrame, baseFrame, yoTime, false, registry, graphicsListRegistry);
positionController = new RigidBodyPositionController(pelvis, elevator, elevator, pelvisFrame, baseFrame, yoTime, false, false, registry, graphicsListRegistry);

defaultHeight = new DoubleParameter(getClass().getSimpleName() + "DefaultHeight", registry);
minHeight = new DoubleParameter(getClass().getSimpleName() + "MinHeight", registry, 0.0);
Expand Down
2 changes: 1 addition & 1 deletion .../ihmc/commonWalkingControlModules/controlModules/pelvis/UserPelvisOrientationManager.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ public UserPelvisOrientationManager(PID3DGainsReadOnly gains, HighLevelHumanoidC
baseFrame = controllerToolbox.getReferenceFrames().getMidFootZUpGroundFrame();
YoDouble yoTime = controllerToolbox.getYoTime();

orientationController = new RigidBodyOrientationController(pelvis, elevator, elevator, baseFrame, yoTime, null, false, registry);
orientationController = new RigidBodyOrientationController(pelvis, elevator, elevator, baseFrame, yoTime, null, false, false, registry);
orientationController.setGains(gains);

homeOrientation = new FrameQuaternion(baseFrame);
Expand Down
88 changes: 85 additions & 3 deletions ...us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ public class RigidBodyControlManager implements SCS2YoGraphicHolder

private final String bodyName;
private final RigidBodyBasics bodyToControl;
private final YoBoolean isImpedanceEnabled;
private final YoRegistry registry;
private final StateMachine<RigidBodyControlMode, RigidBodyControlState> stateMachine;
private final YoEnum<RigidBodyControlMode> requestedState;
Expand Down Expand Up @@ -91,10 +92,13 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl,
Vector3DReadOnly taskspaceLinearWeight,
PID3DGainsReadOnly taskspaceOrientationGains,
PID3DGainsReadOnly taskspacePositionGains,
PID3DGainsReadOnly taskspaceOrientationImpedanceGains,
PID3DGainsReadOnly taskspacePositionImpedanceGains,
ContactablePlaneBody contactableBody,
LoadBearingParameters loadBearingParameters,
RigidBodyControlMode defaultControlMode,
boolean enableFunctionGenerators,
YoBoolean isImpedanceEnabled,
double nominalRhoWeight,
WholeBodyPostureAdjustmentProvider postureAdjustmentProvider,
YoDouble yoTime,
Expand All @@ -103,6 +107,7 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl,
YoRegistry parentRegistry)
{
this.bodyToControl = bodyToControl;
this.isImpedanceEnabled = isImpedanceEnabled;
bodyName = bodyToControl.getName();
String namePrefix = bodyName + "Manager";
registry = new YoRegistry(namePrefix);
Expand All @@ -129,6 +134,7 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl,
yoTime,
jointControlHelper,
enableFunctionGenerators,
isImpedanceEnabled,
parentRegistry);
if (taskspaceOrientationGains == null)
{
Expand All @@ -137,7 +143,7 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl,
taskspaceControlState.setGains(taskspaceOrientationGains);
taskspaceControlState.setWeights(taskspaceAngularWeight);
this.taskspaceControlState = taskspaceControlState;
LogTools.info("Creating manager for " + bodyName + " with orientation controller.");
LogTools.info("Creating manager for " + bodyName + " with orientation controller. (Impedance enabled: " + isImpedanceEnabled.getValue() + ")");
}
else if (taskspaceAngularWeight == null && taskspaceLinearWeight != null)
{
Expand All @@ -148,6 +154,7 @@ else if (taskspaceAngularWeight == null && taskspaceLinearWeight != null)
baseFrame,
yoTime,
enableFunctionGenerators,
isImpedanceEnabled,
parentRegistry,
graphicsListRegistry);
if (taskspacePositionGains == null)
Expand All @@ -157,7 +164,7 @@ else if (taskspaceAngularWeight == null && taskspaceLinearWeight != null)
taskspaceControlState.setGains(taskspacePositionGains);
taskspaceControlState.setWeights(taskspaceLinearWeight);
this.taskspaceControlState = taskspaceControlState;
LogTools.info("Creating manager for " + bodyName + " with position controller.");
LogTools.info("Creating manager for " + bodyName + " with position controller. (Impedance enabled: " + isImpedanceEnabled.getValue() + ")");
}
else if (taskspaceAngularWeight != null && taskspaceLinearWeight != null)
{
Expand All @@ -169,6 +176,7 @@ else if (taskspaceAngularWeight != null && taskspaceLinearWeight != null)
yoTime,
jointControlHelper,
enableFunctionGenerators,
isImpedanceEnabled,
graphicsListRegistry,
registry);
if (taskspaceOrientationGains == null || taskspacePositionGains == null)
Expand All @@ -179,8 +187,10 @@ else if (taskspaceAngularWeight != null && taskspaceLinearWeight != null)
}
taskspaceControlState.setGains(taskspaceOrientationGains, taskspacePositionGains);
taskspaceControlState.setWeights(taskspaceAngularWeight, taskspaceLinearWeight);
if (taskspaceOrientationImpedanceGains != null && taskspacePositionImpedanceGains != null)
taskspaceControlState.setImpedanceGains(taskspaceOrientationImpedanceGains, taskspacePositionImpedanceGains);
this.taskspaceControlState = taskspaceControlState;
LogTools.info("Creating manager for " + bodyName + " with pose controller.");
LogTools.info("Creating manager for " + bodyName + " with pose controller. (Impedance enabled: " + isImpedanceEnabled.getValue() + ")");
}
else
{
Expand Down Expand Up @@ -396,6 +406,73 @@ public void handleHybridTrajectoryCommand(SO3TrajectoryControllerCommand taskspa
}
}

public void handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand taskspaceCommand,
JointspaceTrajectoryCommand jointSpaceCommand,
WrenchTrajectoryControllerCommand feedForwardCommand)
{
computeDesiredJointPositions(initialJointPositions);

if (stateMachine.getCurrentStateKey() == RigidBodyControlMode.LOADBEARING)
{ // If in LOADBEARING mode, execute the trajectory in that state
loadBearingControlState.handleAsOrientationTrajectoryCommand(taskspaceCommand);
loadBearingControlState.handleJointTrajectoryCommand(jointSpaceCommand, initialJointPositions);
}
else if (taskspaceControlState.handleHybridTrajectoryCommand(taskspaceCommand, jointSpaceCommand, feedForwardCommand, initialJointPositions))
{ // Otherwise execute in TASKSPACE mode
requestState(taskspaceControlState.getControlMode());
}
else
{
LogTools.warn(getClass().getSimpleName() + " for " + bodyName + " received invalid hybrid SE3 trajectory command.");
hold();
}
}

public void handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand taskspaceCommand,
JointspaceTrajectoryCommand jointSpaceCommand,
SE3PIDGainsTrajectoryControllerCommand gainsCommand)
{
computeDesiredJointPositions(initialJointPositions);

if (stateMachine.getCurrentStateKey() == RigidBodyControlMode.LOADBEARING)
{ // If in LOADBEARING mode, execute the trajectory in that state
loadBearingControlState.handleAsOrientationTrajectoryCommand(taskspaceCommand);
loadBearingControlState.handleJointTrajectoryCommand(jointSpaceCommand, initialJointPositions);
}
else if (taskspaceControlState.handleHybridTrajectoryCommand(taskspaceCommand, jointSpaceCommand, gainsCommand, initialJointPositions))
{ // Otherwise execute in TASKSPACE mode
requestState(taskspaceControlState.getControlMode());
}
else
{
LogTools.warn(getClass().getSimpleName() + " for " + bodyName + " received invalid hybrid SE3 trajectory command.");
hold();
}
}

public void handleHybridTrajectoryCommand(SE3TrajectoryControllerCommand taskspaceCommand,
JointspaceTrajectoryCommand jointSpaceCommand,
WrenchTrajectoryControllerCommand feedForwardCommand,
SE3PIDGainsTrajectoryControllerCommand gainsCommand)
{
computeDesiredJointPositions(initialJointPositions);

if (stateMachine.getCurrentStateKey() == RigidBodyControlMode.LOADBEARING)
{ // If in LOADBEARING mode, execute the trajectory in that state
loadBearingControlState.handleAsOrientationTrajectoryCommand(taskspaceCommand);
loadBearingControlState.handleJointTrajectoryCommand(jointSpaceCommand, initialJointPositions);
}
else if (taskspaceControlState.handleHybridTrajectoryCommand(taskspaceCommand, jointSpaceCommand, feedForwardCommand, gainsCommand, initialJointPositions))
{ // Otherwise execute in TASKSPACE mode
requestState(taskspaceControlState.getControlMode());
}
else
{
LogTools.warn(getClass().getSimpleName() + " for " + bodyName + " received invalid hybrid SE3 trajectory command.");
hold();
}
}

public void handleDesiredAccelerationsCommand(DesiredAccelerationsCommand command)
{
if (userControlState.handleDesiredAccelerationsCommand(command))
Expand Down Expand Up @@ -688,6 +765,11 @@ public FeedbackControlCommandList createFeedbackControlTemplate()
return ret;
}

public YoBoolean getImpedanceEnabled()
{
return isImpedanceEnabled;
}

public RigidBodyBasics getBodyToControl()
{
return bodyToControl;
Expand Down
Loading
Loading