How to use

us.ihmc.humanoidRobotics.communication.controllerAPI.command.SE3TrajectoryControllerCommand

constructor

public PelvisTrajectoryCommand(Random random)
{
 se3Trajectory = new SE3TrajectoryControllerCommand(random);
}

public PelvisTrajectoryCommand()
{
 se3Trajectory = new SE3TrajectoryControllerCommand();
}

public FootTrajectoryCommand()
{
 se3Trajectory = new SE3TrajectoryControllerCommand();
}

public QuadrupedBodyTrajectoryCommand(Random random)
{
 se3Trajectory = new SE3TrajectoryControllerCommand(random);
}

public QuadrupedBodyTrajectoryCommand()
{
 se3Trajectory = new SE3TrajectoryControllerCommand();
}

public HandTrajectoryCommand(RobotSide robotSide, ReferenceFrame dataFrame, ReferenceFrame trajectoryFrame)
{
 se3Trajectory = new SE3TrajectoryControllerCommand(dataFrame, trajectoryFrame);
 wrenchTrajectory = new WrenchTrajectoryControllerCommand(dataFrame, trajectoryFrame);
 this.robotSide = robotSide;
}

public FootTrajectoryCommand(Random random)
{
 se3Trajectory = new SE3TrajectoryControllerCommand(random);
 robotSide = RobotSide.generateRandomRobotSide(random);
}

public HandTrajectoryCommand()
{
 se3Trajectory = new SE3TrajectoryControllerCommand();
 wrenchTrajectory = new WrenchTrajectoryControllerCommand();
}

public HandTrajectoryCommand(Random random)
{
 se3Trajectory = new SE3TrajectoryControllerCommand(random);
 wrenchTrajectory = new WrenchTrajectoryControllerCommand(random);
 robotSide = RobotSide.generateRandomRobotSide(random);
}

public HandHybridJointspaceTaskspaceTrajectoryCommand(Random random)
{
 this(RobotSide.generateRandomRobotSide(random), new SE3TrajectoryControllerCommand(random), new JointspaceTrajectoryCommand(random));
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSO3()
{
 Random random = new Random(4219L);
 for (int i = 0; i < 100; i++)
 {
   SO3TrajectoryControllerCommand expected = new SO3TrajectoryControllerCommand(random);
   SE3TrajectoryControllerCommand intermediate = new SE3TrajectoryControllerCommand();
   SO3TrajectoryControllerCommand actual = new SO3TrajectoryControllerCommand();
   CommandConversionTools.convertToSE3(expected, intermediate);
   CommandConversionTools.convertToSO3(intermediate, actual);
   Assert.assertEquals(0, intermediate.getSelectionMatrix().getLinearPart().getNumberOfSelectedAxes());
   Assert.assertTrue(expected.epsilonEquals(actual, Double.MIN_VALUE));
 }
}

  @ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testEuclidean()
  {
   Random random = new Random(5204L);
   for (int i = 0; i < 100; i++)
   {
     EuclideanTrajectoryControllerCommand expected = new EuclideanTrajectoryControllerCommand(random);
     SE3TrajectoryControllerCommand intermediate = new SE3TrajectoryControllerCommand();
     EuclideanTrajectoryControllerCommand actual = new EuclideanTrajectoryControllerCommand();

     CommandConversionTools.convertToSE3(expected, intermediate);
     CommandConversionTools.convertToEuclidean(intermediate, actual);

     Assert.assertEquals(0, intermediate.getSelectionMatrix().getAngularPart().getNumberOfSelectedAxes());
     Assert.assertTrue(expected.epsilonEquals(actual, Double.MIN_VALUE));
   }
  }
}

message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weightMatrix.getLinearPart()));
SE3TrajectoryControllerCommand command = new SE3TrajectoryControllerCommand();
command.set(resolver, message);
manager.handleTaskspaceTrajectoryCommand(command);

message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weightMatrix.getLinearPart()));
SE3TrajectoryControllerCommand command = new SE3TrajectoryControllerCommand();
command.set(worldFrame, worldFrame, message);
manager.handleTaskspaceTrajectoryCommand(command);

message.getTaskspaceTrajectoryPoints().add().set(HumanoidMessageTools.createSE3TrajectoryPointMessage(trajectoryTime, position, orientation, linearVelocity, angularVelocity));
SE3TrajectoryControllerCommand command = new SE3TrajectoryControllerCommand();
command.set(worldFrame, worldFrame, message);
manager.handleTaskspaceTrajectoryCommand(command);