Skip to content

Commit 4544f6e

Browse files
calvertdwcalvertdw
authored
Add option to stream spine and leg jointspace from KST (#880)
1 parent 5a6e812 commit 4544f6e

File tree

14 files changed

+561
-29
lines changed

14 files changed

+561
-29
lines changed

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KSTStreamingMessageFactory.java

Lines changed: 48 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,15 +55,19 @@ public class KSTStreamingMessageFactory
5555
private final FloatingJointBasics rootJoint;
5656
private final OneDoFJointBasics[] oneDoFJoints;
5757
private final OneDoFJointBasics[] neckJoints;
58+
private final OneDoFJointBasics[] spineJoints;
5859
private final SideDependentList<OneDoFJointBasics[]> armJoints = new SideDependentList<>();
60+
private final SideDependentList<OneDoFJointBasics[]> legJoints = new SideDependentList<>();
5961

6062
private final YoBoolean enableVelocity = new YoBoolean("enableVelocity", registry);
6163
private final YoBoolean enableAcceleration = new YoBoolean("enableAcceleration", registry);
6264

6365
private final WholeBodyStreamingMessage output = new WholeBodyStreamingMessage();
6466

6567
private final YoJointspaceStreamingMessage yoNeckStreamingMessage;
68+
private final YoJointspaceStreamingMessage yoSpineStreamingMessage;
6669
private final SideDependentList<YoJointspaceStreamingMessage> yoArmStreamingMessages = new SideDependentList<>();
70+
private final SideDependentList<YoJointspaceStreamingMessage> yoLegStreamingMessages = new SideDependentList<>();
6771

6872
private final YoEuclideanStreamingMessage yoCenterOfMassStreamingMessage;
6973
private final YoSE3StreamingMessage yoPelvisStreamingMessage;
@@ -80,6 +84,7 @@ public KSTStreamingMessageFactory(FullHumanoidRobotModelFactory fullRobotModelFa
8084

8185
RigidBodyBasics head = fullRobotModel.getHead();
8286
RigidBodyBasics chest = fullRobotModel.getChest();
87+
RigidBodyBasics pelvis = fullRobotModel.getPelvis();
8388

8489
if (head == null)
8590
{
@@ -92,8 +97,15 @@ public KSTStreamingMessageFactory(FullHumanoidRobotModelFactory fullRobotModelFa
9297
yoNeckStreamingMessage = new YoJointspaceStreamingMessage(neckJoints);
9398
}
9499

100+
spineJoints = MultiBodySystemTools.createOneDoFJointPath(pelvis, chest);
101+
yoSpineStreamingMessage = new YoJointspaceStreamingMessage(spineJoints);
102+
95103
for (RobotSide robotSide : RobotSide.values)
96104
{
105+
RigidBodyBasics foot = fullRobotModel.getFoot(robotSide);
106+
legJoints.put(robotSide, MultiBodySystemTools.createOneDoFJointPath(pelvis, foot));
107+
yoLegStreamingMessages.put(robotSide, new YoJointspaceStreamingMessage(legJoints.get(robotSide)));
108+
97109
RigidBodyBasics hand = fullRobotModel.getHand(robotSide);
98110
if (hand == null)
99111
continue;
@@ -104,7 +116,7 @@ public KSTStreamingMessageFactory(FullHumanoidRobotModelFactory fullRobotModelFa
104116
}
105117

106118
yoCenterOfMassStreamingMessage = new YoEuclideanStreamingMessage("com", registry);
107-
yoPelvisStreamingMessage = new YoSE3StreamingMessage(fullRobotModel.getPelvis(), registry);
119+
yoPelvisStreamingMessage = new YoSE3StreamingMessage(pelvis, registry);
108120
yoChestStreamingMessage = new YoSO3StreamingMessage(chest, registry);
109121

110122
parentRegistry.addChild(registry);
@@ -169,6 +181,27 @@ public void computeArmStreamingMessage(RobotSide robotSide)
169181
output.setHasRightArmStreamingMessage(true);
170182
}
171183

184+
public void computeLegStreamingMessage(RobotSide robotSide)
185+
{
186+
checkIfDataHasBeenSet();
187+
188+
if (legJoints.get(robotSide) == null)
189+
return;
190+
191+
JointspaceStreamingMessage streamingMessage = select(robotSide, output.getLeftLegStreamingMessage(), output.getRightLegStreamingMessage());
192+
computeJointspaceMessage(legJoints.get(robotSide),
193+
enableVelocity.getValue(),
194+
enableAcceleration.getValue(),
195+
output.getStreamIntegrationDuration(),
196+
streamingMessage);
197+
yoLegStreamingMessages.get(robotSide).setFromMessage(streamingMessage);
198+
199+
if (robotSide == RobotSide.LEFT)
200+
output.setHasLeftLegStreamingMessage(true);
201+
else
202+
output.setHasRightLegStreamingMessage(true);
203+
}
204+
172205
public void computeHandStreamingMessages()
173206
{
174207
for (RobotSide robotSide : RobotSide.values)
@@ -224,6 +257,20 @@ public void computeHandStreamingMessage(RobotSide robotSide, long trajectoryFram
224257
output.setHasRightHandStreamingMessage(true);
225258
}
226259

260+
public void computeSpineStreamingMessage()
261+
{
262+
checkIfDataHasBeenSet();
263+
264+
computeJointspaceMessage(spineJoints,
265+
enableVelocity.getValue(),
266+
enableAcceleration.getValue(),
267+
output.getStreamIntegrationDuration(),
268+
output.getSpineStreamingMessage());
269+
yoSpineStreamingMessage.setFromMessage(output.getSpineStreamingMessage());
270+
271+
output.setHasSpineStreamingMessage(true);
272+
}
273+
227274
public void computeNeckStreamingMessage()
228275
{
229276
if (neckJoints == null)

‎ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/kinematicsStreamingToolboxModule/KSTTools.java

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -100,12 +100,14 @@ public class KSTTools
100100

101101
private final KinematicsStreamingToolboxConfigurationCommand configurationCommand = new KinematicsStreamingToolboxConfigurationCommand();
102102
private final KinematicsStreamingToolboxInitialConfigurationCommand initCommand = new KinematicsStreamingToolboxInitialConfigurationCommand();
103+
private final YoBoolean isSpineJointspaceOutputEnabled;
103104
private final YoBoolean isNeckJointspaceOutputEnabled;
104105
private final YoBoolean isChestTaskspaceOutputEnabled;
105106
private final YoBoolean isPelvisTaskspaceOutputEnabled;
106107
private final YoBoolean isCenterOfMassOutputEnabled;
107108
private final SideDependentList<YoBoolean> areHandTaskspaceOutputsEnabled = new SideDependentList<>();
108109
private final SideDependentList<YoBoolean> areArmJointspaceOutputsEnabled = new SideDependentList<>();
110+
private final SideDependentList<YoBoolean> areLegJointspaceOutputsEnabled = new SideDependentList<>();
109111

110112
private final YoBoolean invalidUserInput;
111113
private final YoLong latestInputTimestampSource;
@@ -212,6 +214,7 @@ public KSTTools(CommandInputManager commandInputManager,
212214
previousInputReceivedTime = new YoDouble("previousInputReceivedTime", registry);
213215
flushInputCommands();
214216

217+
isSpineJointspaceOutputEnabled = new YoBoolean("isSpineJointspaceOutputEnabled", registry);
215218
isNeckJointspaceOutputEnabled = new YoBoolean("isNeckJointspaceOutputEnabled", registry);
216219
isChestTaskspaceOutputEnabled = new YoBoolean("isChestTaskspaceOutputEnabled", registry);
217220
isPelvisTaskspaceOutputEnabled = new YoBoolean("isPelvisTaskspaceOutputEnabled", registry);
@@ -223,6 +226,8 @@ public KSTTools(CommandInputManager commandInputManager,
223226
areHandTaskspaceOutputsEnabled.put(robotSide, isHandTaskspaceOutputEnabled);
224227
YoBoolean isArmJointspaceOutputEnabled = new YoBoolean("is" + robotSide.getPascalCaseName() + "ArmJointspaceOutputEnabled", registry);
225228
areArmJointspaceOutputsEnabled.put(robotSide, isArmJointspaceOutputEnabled);
229+
YoBoolean isLegJointspaceOutputEnabled = new YoBoolean("is" + robotSide.getPascalCaseName() + "LegJointspaceOutputEnabled", registry);
230+
areLegJointspaceOutputsEnabled.put(robotSide, isLegJointspaceOutputEnabled);
226231
}
227232

228233
invalidUserInput = new YoBoolean("invalidUserInput", registry);
@@ -242,6 +247,7 @@ public void update()
242247
configurationCommand.set(commandInputManager.pollNewestCommand(KinematicsStreamingToolboxConfigurationCommand.class));
243248
}
244249

250+
isSpineJointspaceOutputEnabled.set(configurationCommand.isSpineJointspaceEnabled());
245251
isNeckJointspaceOutputEnabled.set(configurationCommand.isNeckJointspaceEnabled());
246252
isChestTaskspaceOutputEnabled.set(configurationCommand.isChestTaskspaceEnabled());
247253
isPelvisTaskspaceOutputEnabled.set(configurationCommand.isPelvisTaskspaceEnabled());
@@ -251,6 +257,7 @@ public void update()
251257
{
252258
areHandTaskspaceOutputsEnabled.get(robotSide).set(configurationCommand.isHandTaskspaceEnabled(robotSide));
253259
areArmJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isArmJointspaceEnabled(robotSide));
260+
areLegJointspaceOutputsEnabled.get(robotSide).set(configurationCommand.isLegJointspaceEnabled(robotSide));
254261
}
255262

256263
if (commandInputManager.isNewCommandAvailable(KinematicsStreamingToolboxInitialConfigurationCommand.class))
@@ -441,8 +448,13 @@ public WholeBodyStreamingMessage setupStreamingMessage(KSTOutputDataReadOnly sol
441448

442449
if (areArmJointspaceOutputsEnabled.get(robotSide).getValue())
443450
streamingMessageFactory.computeArmStreamingMessage(robotSide);
451+
452+
if (areLegJointspaceOutputsEnabled.get(robotSide).getValue())
453+
streamingMessageFactory.computeLegStreamingMessage(robotSide);
444454
}
445455

456+
if (isSpineJointspaceOutputEnabled.getValue())
457+
streamingMessageFactory.computeSpineStreamingMessage();
446458
if (isNeckJointspaceOutputEnabled.getValue())
447459
streamingMessageFactory.computeNeckStreamingMessage();
448460
if (isChestTaskspaceOutputEnabled.getValue())

‎ihmc-communication/src/main/java/us/ihmc/communication/controllerAPI/MessageUnpackingTools.java

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -123,6 +123,8 @@ public static MessageUnpacker<WholeBodyStreamingMessage> createWholeBodyStreamin
123123
private final SideDependentList<HandHybridJointspaceTaskspaceTrajectoryMessage> handHybridJointspaceTaskspaceTrajectoryMessages = new SideDependentList<>(
124124
new HandHybridJointspaceTaskspaceTrajectoryMessage(),
125125
new HandHybridJointspaceTaskspaceTrajectoryMessage());
126+
private final SideDependentList<LegTrajectoryMessage> legTrajectoryMessages = new SideDependentList<>(new LegTrajectoryMessage(), new LegTrajectoryMessage());
127+
private final SpineTrajectoryMessage spineTrajectoryMessage = new SpineTrajectoryMessage();
126128
private final ChestTrajectoryMessage chestTrajectoryMessage = new ChestTrajectoryMessage();
127129
private final PelvisTrajectoryMessage pelvisTrajectoryMessage = new PelvisTrajectoryMessage();
128130
private final NeckTrajectoryMessage neckTrajectoryMessage = new NeckTrajectoryMessage();
@@ -163,6 +165,22 @@ public void unpackMessage(WholeBodyStreamingMessage message, List<Settable<?>> m
163165
messagesToPack.add(pelvisTrajectoryMessage);
164166
}
165167

168+
if (message.getHasSpineStreamingMessage())
169+
{
170+
spineTrajectoryMessage.setSequenceId(sequenceId);
171+
spineTrajectoryMessage.setUniqueId(uniqueId);
172+
toJointspaceTrajectoryMessage(message.getSpineStreamingMessage(),
173+
spineTrajectoryMessage.getJointspaceTrajectory(),
174+
sequenceId,
175+
uniqueId,
176+
streamIntegrationDuration,
177+
sourceTimestamp);
178+
messagesToPack.add(spineTrajectoryMessage);
179+
180+
// Disable pelvis taskspace orientation trajectory if spine trajectory is present
181+
MessageTools.packSelectionMatrix3DMessage(false, pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix());
182+
}
183+
166184
if (message.getHasNeckStreamingMessage())
167185
{
168186
neckTrajectoryMessage.setSequenceId(sequenceId);
@@ -250,6 +268,24 @@ else if (hasHandStreamingMessage)
250268
sourceTimestamp);
251269
messagesToPack.add(handTrajectoryMessage);
252270
}
271+
272+
boolean hasLegStreamingMessage = select(robotSide, message.getHasLeftLegStreamingMessage(), message.getHasRightLegStreamingMessage());
273+
JointspaceStreamingMessage legStreamingMessage = select(robotSide, message.getLeftLegStreamingMessage(), message.getRightLegStreamingMessage());
274+
275+
if (hasLegStreamingMessage)
276+
{
277+
LegTrajectoryMessage legTrajectoryMessage = legTrajectoryMessages.get(robotSide);
278+
legTrajectoryMessage.setSequenceId(sequenceId);
279+
legTrajectoryMessage.setUniqueId(uniqueId);
280+
legTrajectoryMessage.setRobotSide(robotSide.toByte());
281+
toJointspaceTrajectoryMessage(legStreamingMessage,
282+
legTrajectoryMessage.getJointspaceTrajectory(),
283+
sequenceId,
284+
uniqueId,
285+
streamIntegrationDuration,
286+
sourceTimestamp);
287+
messagesToPack.add(legTrajectoryMessage);
288+
}
253289
}
254290
}
255291

‎ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/kinematicsStreamingToolboxAPI/KinematicsStreamingToolboxConfigurationCommand.java

Lines changed: 35 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,9 @@ public class KinematicsStreamingToolboxConfigurationCommand
1717

1818
private boolean enableLeftArmJointspace = true;
1919
private boolean enableRightArmJointspace = true;
20+
private boolean enableLeftLegJointspace = true;
21+
private boolean enableRightLegJointspace = true;
22+
private boolean enableSpineJointspace = true;
2023
private boolean enableNeckJointspace = true;
2124

2225
private boolean enableLeftHandTaskspace = true;
@@ -41,14 +44,18 @@ public void clear()
4144

4245
lockPelvis = false;
4346
lockChest = false;
47+
4448
enableLeftArmJointspace = true;
4549
enableRightArmJointspace = true;
46-
enableNeckJointspace = true;
47-
4850
enableLeftHandTaskspace = true;
4951
enableRightHandTaskspace = true;
52+
enableNeckJointspace = true;
5053
enableChestTaskspace = true;
5154
enablePelvisTaskspace = true;
55+
56+
enableLeftLegJointspace = false;
57+
enableRightLegJointspace = false;
58+
enableSpineJointspace = false;
5259
enableCenterOfMassTrajectory = false;
5360

5461
leftHandTrajectoryFrameId = WORLD_FRAME_ID;
@@ -66,6 +73,9 @@ public void set(KinematicsStreamingToolboxConfigurationCommand other)
6673
lockChest = other.lockChest;
6774
enableLeftArmJointspace = other.enableLeftArmJointspace;
6875
enableRightArmJointspace = other.enableRightArmJointspace;
76+
enableLeftLegJointspace = other.enableLeftLegJointspace;
77+
enableRightLegJointspace = other.enableRightLegJointspace;
78+
enableSpineJointspace = other.enableSpineJointspace;
6979
enableNeckJointspace = other.enableNeckJointspace;
7080

7181
enableLeftHandTaskspace = other.enableLeftHandTaskspace;
@@ -92,6 +102,9 @@ public void setFromMessage(KinematicsStreamingToolboxConfigurationMessage messag
92102

93103
enableLeftArmJointspace = message.getEnableLeftArmJointspace();
94104
enableRightArmJointspace = message.getEnableRightArmJointspace();
105+
enableLeftLegJointspace = message.getEnableLeftLegJointspace();
106+
enableRightLegJointspace = message.getEnableRightLegJointspace();
107+
enableSpineJointspace = message.getEnableSpineJointspace();
95108
enableNeckJointspace = message.getEnableNeckJointspace();
96109

97110
enableLeftHandTaskspace = message.getEnableLeftHandTaskspace();
@@ -137,6 +150,26 @@ public boolean isRightArmJointspaceEnabled()
137150
return enableRightArmJointspace;
138151
}
139152

153+
public boolean isEnableLeftLegJointspaceEnabled()
154+
{
155+
return enableLeftLegJointspace;
156+
}
157+
158+
public boolean isEnableRightLegJointspaceEnabled()
159+
{
160+
return enableRightLegJointspace;
161+
}
162+
163+
public boolean isLegJointspaceEnabled(RobotSide robotSide)
164+
{
165+
return robotSide == RobotSide.LEFT ? isEnableLeftLegJointspaceEnabled() : isEnableRightLegJointspaceEnabled();
166+
}
167+
168+
public boolean isSpineJointspaceEnabled()
169+
{
170+
return enableSpineJointspace;
171+
}
172+
140173
public boolean isNeckJointspaceEnabled()
141174
{
142175
return enableNeckJointspace;

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

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,21 @@ module controller_msgs
5454
*/
5555
boolean has_right_arm_streaming_message;
5656
controller_msgs::msg::dds::JointspaceStreamingMessage right_arm_streaming_message;
57+
/**
58+
* Information for the spine joints
59+
*/
60+
boolean has_spine_streaming_message;
61+
controller_msgs::msg::dds::JointspaceStreamingMessage spine_streaming_message;
62+
/**
63+
* Information for the left leg joints
64+
*/
65+
boolean has_left_leg_streaming_message;
66+
controller_msgs::msg::dds::JointspaceStreamingMessage left_leg_streaming_message;
67+
/**
68+
* Information for the right leg joints
69+
*/
70+
boolean has_right_leg_streaming_message;
71+
controller_msgs::msg::dds::JointspaceStreamingMessage right_leg_streaming_message;
5772
/**
5873
* Information for the chest
5974
*/

‎ihmc-interfaces/src/main/generated-idl/toolbox_msgs/msg/KinematicsStreamingToolboxConfigurationMessage_.idl

Lines changed: 22 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -30,23 +30,41 @@ module toolbox_msgs
3030
*/
3131
boolean lock_chest;
3232
/**
33-
* Whether the left arm should be controller in joint-space.
33+
* Whether the left arm should be controlled in joint-space.
3434
* This is compatible with the hand task-space control.
3535
* Default value is true.
3636
*/
3737
@defaultValue(value=True)
3838
boolean enable_left_arm_jointspace;
3939
/**
40-
* Whether the right arm should be controller in joint-space.
40+
* Whether the right arm should be controlled in joint-space.
4141
* This is compatible with the hand task-space control.
4242
* Default value is true.
4343
*/
4444
@defaultValue(value=True)
4545
boolean enable_right_arm_jointspace;
4646
/**
47-
* Whether the neck should be controller in joint-space.
47+
* Whether the spine should be controlled in joint-space.
48+
* This is useful when streaming position control directly to the robot.
49+
* Default value is false.
50+
*/
51+
boolean enable_spine_jointspace;
52+
/**
53+
* Whether the left leg should be controlled in joint-space.
54+
* This is useful when streaming position control directly to the robot.
55+
* Default value is false.
56+
*/
57+
boolean enable_left_leg_jointspace;
58+
/**
59+
* Whether the right leg should be controlled in joint-space.
60+
* This is useful when streaming position control directly to the robot.
61+
* Default value is false.
62+
*/
63+
boolean enable_right_leg_jointspace;
64+
/**
65+
* Whether the neck should be controlled in joint-space.
4866
* Default value is true.
49-
* Whether the neck should be controller in joint-space.
67+
* Whether the neck should be controlled in joint-space.
5068
*/
5169
@defaultValue(value=True)
5270
boolean enable_neck_jointspace;

0 commit comments

Comments
 (0)