IK Streaming improvements for boxing demo #319
Conversation
| * It is intended to be expressed in the local frame of the feet, i.e., it accounts for the robot yaw. | ||
| */ | ||
| // TODO Add API to set this offset. It was only set from the SCS2 visualizer. | ||
| private final YoVector2D centerOfMassOffset = new YoVector2D("centerOfMassOffset", registry); |
There was a problem hiding this comment.
We had to change the logic for boxing. We used to set the desired CoM to hold to the initial CoM position of the robot when enabling the IK. This could lead to unsafe CoM position (close to the support edge), and also it wouldn't update when a foot would slip. Instead now, we keep updating the desired CoM to be in between the feet. This gives more robustness to the robot stance and updates when a foot slips. We also added an offset which we would use to make the robot lunge a little bit forward improving robustness when throwing punches to the punching bag.
| @@ -656,6 +725,7 @@ private void processCapturabilityBasedStatus(CapturabilityBasedStatus capturabil | |||
| { | |||
| Object<Point3D> leftFootSupportPolygon2d = capturabilityBasedStatus.getLeftFootSupportPolygon3d(); | |||
| Object<Point3D> rightFootSupportPolygon2d = capturabilityBasedStatus.getRightFootSupportPolygon3d(); | |||
| activeContactPointPositions.clear(); | |||
There was a problem hiding this comment.
Bugfix: we had a list growing to infinity!
| @@ -117,8 +119,6 @@ public class KinematicsToolboxController extends ToolboxController | |||
| private static final double GLOBAL_PROPORTIONAL_GAIN = 1200.0; | |||
|
|
|||
| private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); | |||
| private static final double DEFAULT_PRIVILEGED_CONFIGURATION_WEIGHT = 0.025; | |||
| private static final double DEFAULT_PRIVILEGED_CONFIGURATION_GAIN = 50.0; | |||
There was a problem hiding this comment.
These are now defined in the KinematicsToolboxPrivilegedConfigurationParameters class
| @@ -792,6 +811,7 @@ public boolean initialize() | |||
| protected boolean initializeInternal() | |||
| { | |||
| firstTick = true; | |||
| controllerCore.initialize(); | |||
There was a problem hiding this comment.
This is to hopefully address a the following issue:
When the IK solver fails to converge, it keeps on failing forever even after restarting the streaming toolbox. Some data must pertain at reinitialization probably feeding a NaN to the QP. Not sure if that'll fix it
| @@ -833,8 +853,6 @@ protected void resetInternalData() | |||
| // By default, always constrain the center of mass according to the current support polygon (if defined). | |||
| enableSupportPolygonConstraint.set(true); | |||
| inverseKinematicsSolution.getSupportRegion().clear(); | |||
| privilegedWeight.set(DEFAULT_PRIVILEGED_CONFIGURATION_WEIGHT); | |||
| privilegedConfigurationGain.set(DEFAULT_PRIVILEGED_CONFIGURATION_GAIN); | |||
There was a problem hiding this comment.
resetInternalData is used in initialization of the IK. I'm not sure resetting these 2 to their default, so overriding whatever the user changed them to, is intuitive.
| @@ -25,7 +26,7 @@ public void doAction(double timeInState) | |||
| { | |||
| if (!ikController.hasBeenInitialized()) | |||
| { | |||
| tools.getCommandInputManager().clearAllCommands(); | |||
| tools.getCommandInputManager().clearCommands(KinematicsStreamingToolboxInputCommand.class); | |||
There was a problem hiding this comment.
Realized that the previous implementation here would clear out any configuration commands, which are typically sent about the time we asking the toolbox to wake up. At the end, we really only want to clear the input messages to make no obsolete desireds dangle around, the rest is fine
| private final YoDouble defaultPelvisMessageLockWeight = new YoDouble("defaultPelvisMessageLockWeight", registry); | ||
| private final YoDouble defaultChestMessageLockWeight = new YoDouble("defaultChestMessageLockWeight", registry); | ||
| private final YoDouble lockPelvisWeight = new YoDouble("lockPelvisWeight", registry); | ||
| private final YoDouble lockChestWeight = new YoDouble("lockChestWeight", registry); |
There was a problem hiding this comment.
Renamed all these to improve clarity.
When no message is received for a body part, that body part will be held in place. That's what these weights are for.
| @@ -115,6 +125,7 @@ public class KSTStreamingState implements State | |||
| */ | |||
| private final Map<RigidBodyBasics, YoDouble> rigidBodyControlStartTimeMap = new HashMap<>(); | |||
| private final YoDouble[] rigidBodyControlStartTimeArray; | |||
| private final YoDouble centerOfMassControlStartTime = new YoDouble("centerOfMassControlStartTime", registry); | |||
There was a problem hiding this comment.
We added the option to control the CoM
| * @param dt the time step. | ||
| * @return the minimum acceleration that can be applied to the joint to reach the minimum joint position limit. | ||
| */ | ||
| public static double computeJointMinAcceleration(double qMin, double qDotMin, double q, double qDot, double dt) |
There was a problem hiding this comment.
We now consider joint velocity limits
|
|
||
| // All the commands that are supported by the IK solver will automatically be forwarded to the IK solver. | ||
| List<Class<? extends Command<?, ?>>> ikSolverCommands = KinematicsToolboxModule.supportedCommands(); | ||
| commandTypesToForward = commandInputManager.getListOfSupportedCommands().stream().filter(ikSolverCommands::contains).toArray(Class[]::new); |
There was a problem hiding this comment.
I believe I've found a new bug that this addresses: we were not forwarding KinematicsToolboxPrivilegedConfigurationCommand nor KinematicsToolboxInitialConfigurationCommand to the IK solver.
This is to address this issue and hopefully prevent a similar issue from happening again. I have not tested this, but it should be safe.
SylvainBertrand
force-pushed
the
feature/ik-streaming-boxing
branch
from
a0662b9
to
c0629cc
Compare
SylvainBertrand
force-pushed
the
feature/ik-streaming-boxing
branch
from
c0629cc
to
cee5a1c
Compare
I extracted the changes from the
feature/multiple-trackerswhich we had used to work on the boxing demo. The original branch is quite a mess, so I wanted to extract the goodies about the IK streaming only.Note that I have not grabbed the new values for IK streaming parameters as I'm not 100% they good for general operation. So this PR should be rather safe to merge in.
@LuigiPenco93 You'll probably want to extract the changes from the
feature/multiple-trackersabout the RDX UI and make a separate PR for that.Feel free to merge this PR while I'm gone or we can merge it when I'm back in September.