Skip to content

Commit f4aeec6

Browse files
rjgriffin42rjgriffin42
committed
set up the high level controller state that calls the external controller
1 parent 6460809 commit f4aeec6

File tree

5 files changed

+329
-5
lines changed

5 files changed

+329
-5
lines changed

‎external-control/src/main/cpp/external-control.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,9 @@
55

66
namespace ihmc
77
{
8-
ExternalControlImpl::ExternalControlImpl(const double default_position, const double default_damping, const int number_of_joints)
8+
ExternalControlImpl::ExternalControlImpl(const double default_stiffness, const double default_damping, const int number_of_joints)
99
{
10-
constant_position_controller_ = ConstantPositionController(default_position, default_damping, number_of_joints);
10+
constant_position_controller_ = ConstantPositionController(default_stiffness, default_damping, number_of_joints);
1111
desired_state_data_ = Eigen::VectorXd(13 + 2 * number_of_joints);
1212
desired_control_data_ = Eigen::VectorXd(number_of_joints);
1313
p_gains_ = Eigen::VectorXd(number_of_joints);

‎external-control/src/main/cpp/external-control.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ namespace ihmc
77
class ExternalControlImpl
88
{
99
public:
10-
explicit ExternalControlImpl(const double default_position, const double default_damping, const int number_of_joints);
10+
explicit ExternalControlImpl(const double default_stiffness, const double default_damping, const int number_of_joints);
1111

1212
virtual ~ExternalControlImpl() = default;
1313

‎external-control/src/main/generated-java/us/ihmc/externalControl/ExternalControlWrapper.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@ public class ExternalControlWrapper extends us.ihmc.externalControl.presets.Exte
1919
/** Pointer cast constructor. Invokes {@link Pointer#Pointer(Pointer)}. */
2020
public ExternalControlImpl(Pointer p) { super(p); }
2121

22-
public ExternalControlImpl(double default_position, double default_damping, int number_of_joints) { super((Pointer)null); allocate(default_position, default_damping, number_of_joints); }
23-
private native void allocate(double default_position, double default_damping, int number_of_joints);
22+
public ExternalControlImpl(double default_stiffness, double default_damping, int number_of_joints) { super((Pointer)null); allocate(default_stiffness, default_damping, number_of_joints); }
23+
private native void allocate(double default_stiffness, double default_damping, int number_of_joints);
2424

2525

2626

‎external-control/src/main/java/us/ihmc/externalControl/ExternalControl.java‎

Lines changed: 170 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,170 @@
1+
package us.ihmc.externalControl;
2+
3+
import org.ejml.data.DMatrixRMaj;
4+
import us.ihmc.euclid.referenceFrame.FramePose3D;
5+
import us.ihmc.euclid.referenceFrame.FrameVector3D;
6+
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
7+
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointReadOnly;
8+
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
9+
import us.ihmc.robotics.robotSide.RobotSide;
10+
import us.ihmc.robotics.robotSide.SideDependentList;
11+
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputBasics;
12+
13+
import java.util.HashMap;
14+
15+
public class ExternalControl
16+
{
17+
private final ExternalControlWrapper.ExternalControlImpl externalControlImpl;
18+
private final DMatrixRMaj robotState;
19+
private final DMatrixRMaj robotControl;
20+
private final DMatrixRMaj feetPositions;
21+
private boolean leftInContact;
22+
private boolean rightInContact;
23+
private final DMatrixRMaj solutionRobotState;
24+
private final DMatrixRMaj solutionTorqueVector;
25+
private final DMatrixRMaj solutionStiffnessVector;
26+
private final DMatrixRMaj solutionDampingVector;
27+
private final RigidBodyBasics baseBody;
28+
private final OneDoFJointReadOnly[] joints;
29+
private final FramePose3D basePose = new FramePose3D();
30+
private final FrameVector3D tempVector = new FrameVector3D();
31+
private final FrameVector3D tempPoint = new FrameVector3D();
32+
private final FramePose3D solutionBasePose = new FramePose3D();
33+
private final HashMap<OneDoFJointReadOnly, SolutionJointData> solutionJointData = new HashMap<>();
34+
35+
public ExternalControl(RigidBodyBasics baseBody, OneDoFJointReadOnly[] joints, double defaultStiffness, double defaultDamping)
36+
{
37+
this.baseBody = baseBody;
38+
this.joints = joints;
39+
robotState = new DMatrixRMaj(2 * joints.length + 13, 1);
40+
robotControl = new DMatrixRMaj(joints.length, 1);
41+
feetPositions = new DMatrixRMaj(6, 1);
42+
solutionRobotState = new DMatrixRMaj(2 * joints.length + 13, 1);
43+
solutionTorqueVector = new DMatrixRMaj(joints.length, 1);
44+
solutionStiffnessVector = new DMatrixRMaj(joints.length, 1);
45+
solutionDampingVector = new DMatrixRMaj(joints.length, 1);
46+
for (OneDoFJointReadOnly joint : joints)
47+
solutionJointData.put(joint, new SolutionJointData());
48+
externalControlImpl = new ExternalControlWrapper.ExternalControlImpl(defaultStiffness, defaultDamping, joints.length);
49+
}
50+
51+
public void setFootStates(SideDependentList<? extends ReferenceFrame> soleFrames, boolean leftInContact, boolean rightInContact)
52+
{
53+
int start = 0;
54+
for (RobotSide robotSide : RobotSide.values)
55+
{
56+
tempPoint.setToZero(soleFrames.get(robotSide));
57+
tempPoint.changeFrame(ReferenceFrame.getWorldFrame());
58+
tempPoint.get(start, feetPositions);
59+
start += 3;
60+
}
61+
this.leftInContact = leftInContact;
62+
this.rightInContact = rightInContact;
63+
}
64+
65+
public void writeRobotState(double currentTime, int hardwareStatus)
66+
{
67+
setRobotState();
68+
setRobotControl();
69+
if (!externalControlImpl.updateRobotState(currentTime,
70+
robotState.data,
71+
robotState.getNumRows(),
72+
robotControl.data,
73+
robotControl.getNumRows(),
74+
leftInContact,
75+
rightInContact,
76+
feetPositions.data,
77+
feetPositions.getNumRows(),
78+
hardwareStatus))
79+
throw new RuntimeException("Failed to successfully write the hardware state across the barrier.");
80+
}
81+
82+
public void readControlSolution()
83+
{
84+
if (!externalControlImpl.getSolution(solutionRobotState.data,
85+
solutionRobotState.numRows,
86+
solutionTorqueVector.data,
87+
solutionTorqueVector.numRows,
88+
solutionStiffnessVector.data,
89+
solutionStiffnessVector.numRows,
90+
solutionDampingVector.data,
91+
solutionDampingVector.numRows))
92+
{
93+
throw new RuntimeException("Failed to retrieve solution data.");
94+
}
95+
solutionBasePose.getPosition().set(solutionRobotState);
96+
solutionBasePose.getOrientation().set(3, 0, solutionRobotState);
97+
int positionStart = 7;
98+
int velocityStart = 7 + 6 + joints.length;
99+
for (int i = 0; i < joints.length; i++)
100+
{
101+
SolutionJointData data = solutionJointData.get(joints[i]);
102+
data.desiredPosition = solutionRobotState.get(positionStart + i, 0);
103+
data.desiredVelocity = solutionRobotState.get(velocityStart + i, 0);
104+
data.torque = solutionTorqueVector.get(i, 0);
105+
data.stiffness = solutionStiffnessVector.get(i, 0);
106+
data.damping = solutionDampingVector.get(i, 0);
107+
}
108+
}
109+
110+
public SolutionJointData getSolutionData(OneDoFJointReadOnly joint)
111+
{
112+
return solutionJointData.get(joint);
113+
}
114+
115+
private void setRobotState()
116+
{
117+
basePose.setToZero(baseBody.getBodyFixedFrame());
118+
basePose.changeFrame(ReferenceFrame.getWorldFrame());
119+
// set the configuration state for the robot
120+
basePose.getPosition().get(0, robotState);
121+
basePose.getOrientation().get(3, robotState);
122+
int start = 7;
123+
for (int i = 0; i < joints.length; i++)
124+
{
125+
robotState.set(start + i, joints[i].getQ());
126+
}
127+
// set linear velocity in body frame
128+
start += joints.length;
129+
tempVector.setIncludingFrame(baseBody.getBodyFixedFrame().getTwistOfFrame().getLinearPart());
130+
tempVector.changeFrame(baseBody.getBodyFixedFrame());
131+
tempVector.get(start, robotState);
132+
// set angular velocity in body frame
133+
start += 3;
134+
tempVector.setIncludingFrame(baseBody.getBodyFixedFrame().getTwistOfFrame().getAngularPart());
135+
tempVector.changeFrame(baseBody.getBodyFixedFrame());
136+
tempVector.get(start, robotState);
137+
// set joint velocity
138+
start += 3;
139+
for (int i = 0; i < joints.length; i++)
140+
{
141+
robotState.set(start + i, joints[i].getQd());
142+
}
143+
}
144+
145+
private void setRobotControl()
146+
{
147+
for (int i = 0; i < joints.length; i++)
148+
{
149+
robotControl.set(i, joints[i].getTau());
150+
}
151+
}
152+
153+
public static class SolutionJointData
154+
{
155+
double desiredPosition;
156+
double desiredVelocity;
157+
double stiffness;
158+
double damping;
159+
double torque;
160+
161+
public void getJointDesiredOutput(JointDesiredOutputBasics jointDesiredOutputToPack)
162+
{
163+
jointDesiredOutputToPack.setDesiredPosition(desiredPosition);
164+
jointDesiredOutputToPack.setDesiredVelocity(desiredVelocity);
165+
jointDesiredOutputToPack.setDesiredTorque(torque);
166+
jointDesiredOutputToPack.setStiffness(stiffness);
167+
jointDesiredOutputToPack.setDamping(damping);
168+
}
169+
}
170+
}

‎external-control/src/main/java/us/ihmc/externalControl/ExternalControllerState.java‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
package us.ihmc.externalControl;
2+
3+
import us.ihmc.commonWalkingControlModules.controllerCore.command.lowLevel.LowLevelOneDoFJointDesiredDataHolder;
4+
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.HighLevelControllerState;
5+
import us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.JointControlBlender;
6+
import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox;
7+
import us.ihmc.commons.MathTools;
8+
import us.ihmc.commons.lists.PairList;
9+
import us.ihmc.communication.controllerAPI.CommandInputManager;
10+
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HighLevelControllerName;
11+
import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
12+
import us.ihmc.robotics.robotSide.RobotSide;
13+
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputBasics;
14+
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputListReadOnly;
15+
import us.ihmc.sensorProcessing.outputData.JointDesiredOutputReadOnly;
16+
import us.ihmc.yoVariables.registry.YoRegistry;
17+
import us.ihmc.yoVariables.variable.YoDouble;
18+
19+
public class ExternalControllerState extends HighLevelControllerState
20+
{
21+
public static boolean REDUCE_YOVARIABLES = false;
22+
23+
private final YoDouble blendRatioCurrentValue;
24+
25+
private final PairList<OneDoFJointBasics, JointControlBlender> jointCommandBlenders = new PairList<>();
26+
private final JointDesiredOutputListReadOnly highLevelControllerOutput;
27+
28+
private final LowLevelOneDoFJointDesiredDataHolder lowLevelOneDoFJointDesiredDataHolder = new LowLevelOneDoFJointDesiredDataHolder();
29+
private final LowLevelOneDoFJointDesiredDataHolder frozenJointDataHolder = new LowLevelOneDoFJointDesiredDataHolder();
30+
private final LowLevelOneDoFJointDesiredDataHolder externalJointDataHolder = new LowLevelOneDoFJointDesiredDataHolder();
31+
32+
private final CommandInputManager commandInputManager;
33+
34+
private final PairList<OneDoFJointBasics, YoDouble> frozenJointDesireds = new PairList<>();
35+
36+
private final HighLevelHumanoidControllerToolbox controllerToolbox;
37+
38+
private final ExternalControl externalControl;
39+
40+
public ExternalControllerState(String namePrefix,
41+
HighLevelControllerName controllerState,
42+
HighLevelHumanoidControllerToolbox controllerToolbox,
43+
OneDoFJointBasics[] controlledJoints,
44+
JointDesiredOutputListReadOnly highLevelControllerOutput,
45+
CommandInputManager commandInputManager)
46+
{
47+
super(namePrefix, controllerState, controlledJoints);
48+
49+
this.highLevelControllerOutput = highLevelControllerOutput;
50+
this.controllerToolbox = controllerToolbox;
51+
52+
this.commandInputManager = commandInputManager;
53+
54+
blendRatioCurrentValue = new YoDouble(namePrefix + "BlendRatioCurrentValue", registry);
55+
56+
externalControl = new ExternalControl(controllerToolbox.getFullRobotModel().getRootBody(), controlledJoints, 100.0, 25.0);
57+
58+
lowLevelOneDoFJointDesiredDataHolder.registerJointsWithEmptyData(controlledJoints);
59+
frozenJointDataHolder.registerJointsWithEmptyData(controlledJoints);
60+
externalJointDataHolder.registerJointsWithEmptyData(controlledJoints);
61+
62+
YoRegistry registryForBlenders = REDUCE_YOVARIABLES ? null : registry;
63+
64+
for (OneDoFJointBasics controlledJoint : controlledJoints)
65+
{
66+
String jointName = controlledJoint.getName();
67+
68+
YoDouble freezePosition = new YoDouble(jointName + "_frozen_qDesired", registry);
69+
freezePosition.setToNaN();
70+
frozenJointDesireds.add(controlledJoint, freezePosition);
71+
72+
JointControlBlender jointControlBlender = new JointControlBlender("_ExternalBlender", controlledJoint, registryForBlenders);
73+
jointCommandBlenders.add(controlledJoint, jointControlBlender);
74+
}
75+
}
76+
77+
@Override
78+
public void onEntry()
79+
{
80+
for (int jointIndex = 0; jointIndex < frozenJointDesireds.size(); jointIndex++)
81+
{
82+
OneDoFJointBasics joint = frozenJointDesireds.get(jointIndex).getLeft();
83+
YoDouble setpoint = frozenJointDesireds.get(jointIndex).getRight();
84+
JointDesiredOutputReadOnly lowLevelJointData = highLevelControllerOutput.getJointDesiredOutput(joint);
85+
if (lowLevelJointData != null && lowLevelJointData.hasDesiredPosition())
86+
setpoint.set(lowLevelJointData.getDesiredPosition());
87+
else
88+
setpoint.set(joint.getQ());
89+
}
90+
91+
blendRatioCurrentValue.set(0.0);
92+
93+
commandInputManager.clearAllCommands();
94+
commandInputManager.setEnabled(false);
95+
}
96+
97+
@Override
98+
public void doAction(double timeInState)
99+
{
100+
controllerToolbox.update();
101+
102+
externalControl.setFootStates(controllerToolbox.getReferenceFrames().getSoleFrames(),
103+
controllerToolbox.getFootSwitches().get(RobotSide.LEFT).hasFootHitGroundFiltered(),
104+
controllerToolbox.getFootSwitches().get(RobotSide.RIGHT).hasFootHitGroundFiltered());
105+
externalControl.writeRobotState(controllerToolbox.getYoTime().getDoubleValue(), 0);
106+
107+
externalControl.readControlSolution();
108+
109+
110+
double gainRatio = blendRatioCurrentValue.getValue();
111+
112+
if (Double.isNaN(gainRatio))
113+
gainRatio = 0.0;
114+
else
115+
gainRatio = MathTools.clamp(gainRatio, 0.0, 1.0);
116+
117+
for (int jointIndex = 0; jointIndex < jointCommandBlenders.size(); jointIndex++)
118+
{
119+
OneDoFJointBasics joint = jointCommandBlenders.get(jointIndex).getLeft();
120+
121+
JointDesiredOutputBasics externalJointData = externalJointDataHolder.getJointDesiredOutput(joint);
122+
externalControl.getSolutionData(joint).getJointDesiredOutput(externalJointData);
123+
124+
YoDouble frozenDesiredPosition = frozenJointDesireds.get(jointIndex).getRight();
125+
126+
JointDesiredOutputBasics frozenJointData = frozenJointDataHolder.getJointDesiredOutput(joint);
127+
frozenJointData.clear();
128+
frozenJointData.setDesiredPosition(frozenDesiredPosition.getDoubleValue());
129+
frozenJointData.setDesiredVelocity(0.0);
130+
frozenJointData.setDesiredAcceleration(0.0);
131+
132+
JointDesiredOutputBasics lowLevelJointData = lowLevelOneDoFJointDesiredDataHolder.getJointDesiredOutput(joint);
133+
lowLevelJointData.clear();
134+
135+
JointControlBlender jointControlBlender = jointCommandBlenders.get(jointIndex).getRight();
136+
jointControlBlender.computeAndUpdateJointControl(lowLevelJointData,
137+
frozenJointData,
138+
externalJointData,
139+
gainRatio);
140+
}
141+
}
142+
143+
@Override
144+
public void onExit(double timeInState)
145+
{
146+
commandInputManager.setEnabled(true);
147+
}
148+
149+
@Override
150+
public JointDesiredOutputListReadOnly getOutputForLowLevelController()
151+
{
152+
return lowLevelOneDoFJointDesiredDataHolder;
153+
}
154+
}

0 commit comments

Comments
 (0)