Skip to content

Commit 8a91523

Browse files
LuigiPenco93LuigiPenco93
committed
added flag for impedance in message hybrid traj
1 parent c47ada6 commit 8a91523

File tree

11 files changed

+92
-5
lines changed

11 files changed

+92
-5
lines changed

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/controlModules/rigidBody/RigidBodyControlManager.java‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@ public class RigidBodyControlManager implements SCS2YoGraphicHolder
5858

5959
private final String bodyName;
6060
private final RigidBodyBasics bodyToControl;
61+
private final YoBoolean isImpedanceEnabled;
6162
private final YoRegistry registry;
6263
private final StateMachine<RigidBodyControlMode, RigidBodyControlState> stateMachine;
6364
private final YoEnum<RigidBodyControlMode> requestedState;
@@ -106,6 +107,7 @@ public RigidBodyControlManager(RigidBodyBasics bodyToControl,
106107
YoRegistry parentRegistry)
107108
{
108109
this.bodyToControl = bodyToControl;
110+
this.isImpedanceEnabled = isImpedanceEnabled;
109111
bodyName = bodyToControl.getName();
110112
String namePrefix = bodyName + "Manager";
111113
registry = new YoRegistry(namePrefix);
@@ -763,6 +765,11 @@ public FeedbackControlCommandList createFeedbackControlTemplate()
763765
return ret;
764766
}
765767

768+
public YoBoolean getImpedanceEnabled()
769+
{
770+
return isImpedanceEnabled;
771+
}
772+
766773
public RigidBodyBasics getBodyToControl()
767774
{
768775
return bodyToControl;

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/WalkingCommandConsumer.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -413,6 +413,7 @@ public void consumeManipulationCommands(WalkingState currentState, boolean allow
413413
HandHybridJointspaceTaskspaceTrajectoryCommand command = handHybridCommands.get(i);
414414
RobotSide robotSide = command.getRobotSide();
415415
RigidBodyControlManager handManager = handManagers.get(robotSide);
416+
handManager.getImpedanceEnabled().set(command.getImpedanceEnabled());
416417
if (handManager != null && (allowCommand || command.getForceExecution()))
417418
{
418419
SE3TrajectoryControllerCommand taskspaceTrajectoryCommand = command.getTaskspaceTrajectoryCommand();

‎ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmControlMode.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,5 +2,5 @@
22

33
public enum RDXArmControlMode
44
{
5-
JOINTSPACE, TASKSPACE, HYBRID
5+
JOINTSPACE, TASKSPACE, HYBRID, HYBRID_IMPEDANCE
66
}

‎ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -249,6 +249,10 @@ public void renderImGuiWidgets()
249249
{
250250
armControlMode = RDXArmControlMode.HYBRID;
251251
}
252+
if (ImGui.radioButton(labels.get("Hybrid Impedance"), armControlMode == RDXArmControlMode.HYBRID_IMPEDANCE))
253+
{
254+
armControlMode = RDXArmControlMode.HYBRID_IMPEDANCE;
255+
}
252256

253257
ImGui.text("Taskspace trajectory frame:");
254258
ImGui.sameLine();
@@ -388,10 +392,11 @@ public void executeDesiredArmCommand(RobotSide robotSide)
388392
RDXBaseUI.pushNotification("Commanding taskspace %s frame trajectory...".formatted(taskspaceTrajectoryFrame.getName()));
389393
communicationHelper.publishToController(handTrajectoryMessage);
390394
}
391-
case HYBRID ->
395+
case HYBRID, HYBRID_IMPEDANCE ->
392396
{
393397
HandHybridJointspaceTaskspaceTrajectoryMessage handHybridJointspaceTaskspaceTrajectoryMessage
394398
= new HandHybridJointspaceTaskspaceTrajectoryMessage();
399+
handHybridJointspaceTaskspaceTrajectoryMessage.setImpedanceEnabled(armControlMode == RDXArmControlMode.HYBRID_IMPEDANCE);
395400
handHybridJointspaceTaskspaceTrajectoryMessage.setRobotSide(robotSide.toByte());
396401
handHybridJointspaceTaskspaceTrajectoryMessage.getTaskspaceTrajectoryMessage().set(se3TrajectoryMessage);
397402
handHybridJointspaceTaskspaceTrajectoryMessage.getJointspaceTrajectoryMessage().set(jointspaceTrajectoryMessage);

‎ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/teleoperation/RDXTeleoperationManager.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -403,8 +403,10 @@ else if (hasEitherArm)
403403
{
404404
for (RobotSide side : interactableHands.sides())
405405
{
406-
desiredRobot.setArmShowing(side, !interactableHands.get(side).isDeleted()
407-
&& (armManager.getArmControlMode() == RDXArmControlMode.JOINTSPACE || armManager.getArmControlMode() == RDXArmControlMode.HYBRID));
406+
desiredRobot.setArmShowing(side,
407+
!interactableHands.get(side).isDeleted() && (armManager.getArmControlMode() == RDXArmControlMode.JOINTSPACE
408+
|| armManager.getArmControlMode() == RDXArmControlMode.HYBRID
409+
|| armManager.getArmControlMode() == RDXArmControlMode.HYBRID_IMPEDANCE));
408410
desiredRobot.setArmColor(side, RDXIKSolverColors.getColor(armManager.getArmIKSolvers().get(side).getQuality()));
409411
}
410412
}

‎ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/HandHybridJointspaceTaskspaceTrajectoryCommand.java‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ public class HandHybridJointspaceTaskspaceTrajectoryCommand
1515
private long sequenceId;
1616
private RobotSide robotSide;
1717
private boolean forceExecution = false;
18+
private boolean impedanceEnabled = false;
1819
private final JointspaceTrajectoryCommand jointspaceTrajectoryCommand = new JointspaceTrajectoryCommand();
1920
private final SE3TrajectoryControllerCommand taskspaceTrajectoryCommand = new SE3TrajectoryControllerCommand();
2021
private final WrenchTrajectoryControllerCommand feedForwardTrajectoryCommand = new WrenchTrajectoryControllerCommand();
@@ -24,6 +25,16 @@ public HandHybridJointspaceTaskspaceTrajectoryCommand()
2425
{
2526
}
2627

28+
public HandHybridJointspaceTaskspaceTrajectoryCommand(RobotSide robotSide, boolean forceExecution, boolean impedanceEnabled, SE3TrajectoryControllerCommand taskspaceTrajectoryCommand,
29+
JointspaceTrajectoryCommand jointspaceTrajectoryCommand)
30+
{
31+
this.robotSide = robotSide;
32+
this.setForceExecution(forceExecution);
33+
this.impedanceEnabled = impedanceEnabled;
34+
this.jointspaceTrajectoryCommand.set(jointspaceTrajectoryCommand);
35+
this.taskspaceTrajectoryCommand.set(taskspaceTrajectoryCommand);
36+
}
37+
2738
public HandHybridJointspaceTaskspaceTrajectoryCommand(RobotSide robotSide, boolean forceExecution, SE3TrajectoryControllerCommand taskspaceTrajectoryCommand,
2839
JointspaceTrajectoryCommand jointspaceTrajectoryCommand)
2940
{
@@ -86,6 +97,7 @@ public void clear()
8697
sequenceId = 0;
8798
robotSide = null;
8899
setForceExecution(false);
100+
setImpedanceEnabled(false);
89101
jointspaceTrajectoryCommand.clear();
90102
feedForwardTrajectoryCommand.clear();
91103
pidGainsTrajectoryCommand.clear();
@@ -103,6 +115,7 @@ public void set(ReferenceFrameHashCodeResolver resolver, HandHybridJointspaceTas
103115
sequenceId = message.getSequenceId();
104116
robotSide = RobotSide.fromByte(message.getRobotSide());
105117
setForceExecution(message.getForceExecution());
118+
setImpedanceEnabled(message.getImpedanceEnabled());
106119
jointspaceTrajectoryCommand.setFromMessage(message.getJointspaceTrajectoryMessage());
107120
taskspaceTrajectoryCommand.set(resolver, message.getTaskspaceTrajectoryMessage());
108121
feedForwardTrajectoryCommand.set(resolver, message.getFeedforwardTaskspaceTrajectoryMessage());
@@ -122,6 +135,7 @@ public void set(HandHybridJointspaceTaskspaceTrajectoryCommand other)
122135
sequenceId = other.sequenceId;
123136
robotSide = other.robotSide;
124137
setForceExecution(other.getForceExecution());
138+
setImpedanceEnabled(other.getImpedanceEnabled());
125139
taskspaceTrajectoryCommand.set(other.getTaskspaceTrajectoryCommand());
126140
jointspaceTrajectoryCommand.set(other.getJointspaceTrajectoryCommand());
127141
feedForwardTrajectoryCommand.set(other.getFeedForwardTrajectoryCommand());
@@ -143,6 +157,16 @@ public void setForceExecution(boolean forceExecution)
143157
this.forceExecution = forceExecution;
144158
}
145159

160+
public boolean getImpedanceEnabled()
161+
{
162+
return impedanceEnabled;
163+
}
164+
165+
public void setImpedanceEnabled(boolean impedanceEnabled)
166+
{
167+
this.impedanceEnabled = impedanceEnabled;
168+
}
169+
146170
public JointspaceTrajectoryCommand getJointspaceTrajectoryCommand()
147171
{
148172
return jointspaceTrajectoryCommand;

‎ihmc-interfaces/src/main/generated-idl/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage_.idl‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,10 @@ module controller_msgs
3535
* To by-pass the safety check and force the execution of this message, set this field to true.
3636
*/
3737
boolean force_execution;
38+
/**
39+
* Whether impedance control should be enabled or not.
40+
*/
41+
boolean impedance_enabled;
3842
/**
3943
* Specifies the side of the robot that will execute the trajectory.
4044
*/

‎ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessage.java‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,10 @@ public class HandHybridJointspaceTaskspaceTrajectoryMessage extends Packet<HandH
2424
* To by-pass the safety check and force the execution of this message, set this field to true.
2525
*/
2626
public boolean force_execution_;
27+
/**
28+
* Whether impedance control should be enabled or not.
29+
*/
30+
public boolean impedance_enabled_;
2731
/**
2832
* Specifies the side of the robot that will execute the trajectory.
2933
*/
@@ -66,6 +70,8 @@ public void set(HandHybridJointspaceTaskspaceTrajectoryMessage other)
6670

6771
force_execution_ = other.force_execution_;
6872

73+
impedance_enabled_ = other.impedance_enabled_;
74+
6975
robot_side_ = other.robot_side_;
7076

7177
ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType.staticCopy(other.taskspace_trajectory_message_, taskspace_trajectory_message_);
@@ -106,6 +112,21 @@ public boolean getForceExecution()
106112
return force_execution_;
107113
}
108114

115+
/**
116+
* Whether impedance control should be enabled or not.
117+
*/
118+
public void setImpedanceEnabled(boolean impedance_enabled)
119+
{
120+
impedance_enabled_ = impedance_enabled;
121+
}
122+
/**
123+
* Whether impedance control should be enabled or not.
124+
*/
125+
public boolean getImpedanceEnabled()
126+
{
127+
return impedance_enabled_;
128+
}
129+
109130
/**
110131
* Specifies the side of the robot that will execute the trajectory.
111132
*/
@@ -180,6 +201,8 @@ public boolean epsilonEquals(HandHybridJointspaceTaskspaceTrajectoryMessage othe
180201

181202
if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.force_execution_, other.force_execution_, epsilon)) return false;
182203

204+
if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.impedance_enabled_, other.impedance_enabled_, epsilon)) return false;
205+
183206
if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.robot_side_, other.robot_side_, epsilon)) return false;
184207

185208
if (!this.taskspace_trajectory_message_.epsilonEquals(other.taskspace_trajectory_message_, epsilon)) return false;
@@ -203,6 +226,8 @@ public boolean equals(Object other)
203226

204227
if(this.force_execution_ != otherMyClass.force_execution_) return false;
205228

229+
if(this.impedance_enabled_ != otherMyClass.impedance_enabled_) return false;
230+
206231
if(this.robot_side_ != otherMyClass.robot_side_) return false;
207232

208233
if (!this.taskspace_trajectory_message_.equals(otherMyClass.taskspace_trajectory_message_)) return false;
@@ -223,6 +248,8 @@ public java.lang.String toString()
223248
builder.append(this.sequence_id_); builder.append(", ");
224249
builder.append("force_execution=");
225250
builder.append(this.force_execution_); builder.append(", ");
251+
builder.append("impedance_enabled=");
252+
builder.append(this.impedance_enabled_); builder.append(", ");
226253
builder.append("robot_side=");
227254
builder.append(this.robot_side_); builder.append(", ");
228255
builder.append("taskspace_trajectory_message=");

‎ihmc-interfaces/src/main/generated-java/controller_msgs/msg/dds/HandHybridJointspaceTaskspaceTrajectoryMessagePubSubType.java‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ public class HandHybridJointspaceTaskspaceTrajectoryMessagePubSubType implements
1515
@Override
1616
public final java.lang.String getDefinitionChecksum()
1717
{
18-
return "57f0562a4836d1e7fb01985aa69986ae3d8fe5c608c96cbac28f49436ec2445d";
18+
return "116a91d7d75006460a92258a598788b7ef72db1c6c9edd05f3928d6cac1acfea";
1919
}
2020

2121
@Override
@@ -58,6 +58,8 @@ public static int getMaxCdrSerializedSize(int current_alignment)
5858

5959
current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);
6060

61+
current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);
62+
6163
current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType.getMaxCdrSerializedSize(current_alignment);
6264

6365
current_alignment += controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.getMaxCdrSerializedSize(current_alignment);
@@ -88,6 +90,9 @@ public final static int getCdrSerializedSize(controller_msgs.msg.dds.HandHybridJ
8890
current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);
8991

9092

93+
current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1);
94+
95+
9196
current_alignment += ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType.getCdrSerializedSize(data.getTaskspaceTrajectoryMessage(), current_alignment);
9297

9398
current_alignment += controller_msgs.msg.dds.JointspaceTrajectoryMessagePubSubType.getCdrSerializedSize(data.getJointspaceTrajectoryMessage(), current_alignment);
@@ -106,6 +111,8 @@ public static void write(controller_msgs.msg.dds.HandHybridJointspaceTaskspaceTr
106111

107112
cdr.write_type_7(data.getForceExecution());
108113

114+
cdr.write_type_7(data.getImpedanceEnabled());
115+
109116
cdr.write_type_9(data.getRobotSide());
110117

111118
ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType.write(data.getTaskspaceTrajectoryMessage(), cdr);
@@ -120,6 +127,8 @@ public static void read(controller_msgs.msg.dds.HandHybridJointspaceTaskspaceTra
120127

121128
data.setForceExecution(cdr.read_type_7());
122129

130+
data.setImpedanceEnabled(cdr.read_type_7());
131+
123132
data.setRobotSide(cdr.read_type_9());
124133

125134
ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType.read(data.getTaskspaceTrajectoryMessage(), cdr);
@@ -134,6 +143,7 @@ public final void serialize(controller_msgs.msg.dds.HandHybridJointspaceTaskspac
134143
{
135144
ser.write_type_4("sequence_id", data.getSequenceId());
136145
ser.write_type_7("force_execution", data.getForceExecution());
146+
ser.write_type_7("impedance_enabled", data.getImpedanceEnabled());
137147
ser.write_type_9("robot_side", data.getRobotSide());
138148
ser.write_type_a("taskspace_trajectory_message", new ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType(), data.getTaskspaceTrajectoryMessage());
139149

@@ -150,6 +160,7 @@ public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, controller_
150160
{
151161
data.setSequenceId(ser.read_type_4("sequence_id"));
152162
data.setForceExecution(ser.read_type_7("force_execution"));
163+
data.setImpedanceEnabled(ser.read_type_7("impedance_enabled"));
153164
data.setRobotSide(ser.read_type_9("robot_side"));
154165
ser.read_type_a("taskspace_trajectory_message", new ihmc_common_msgs.msg.dds.SE3TrajectoryMessagePubSubType(), data.getTaskspaceTrajectoryMessage());
155166

‎ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/HandHybridJointspaceTaskspaceTrajectoryMessage.msg‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,9 @@ uint32 sequence_id
1212
# To by-pass the safety check and force the execution of this message, set this field to true.
1313
bool force_execution false
1414

15+
# Whether impedance control should be enabled or not.
16+
bool impedance_enabled false
17+
1518
# Specifies the side of the robot that will execute the trajectory.
1619
byte robot_side 255
1720
# The taskspace trajectory information.

0 commit comments

Comments
 (0)