@Override public void set(JointspaceVelocityCommand other) { clear(); for (int i = 0; i < other.getNumberOfJoints(); i++) { joints.add(other.joints.get(i)); jointNames.add(other.jointNames.get(i)); } desiredVelocities.set(other.desiredVelocities); weight = other.weight; }
int taskSize = ScrewTools.computeDegreesOfFreedom(commandToConvert.getJoints()); motionQPInputToPack.setIsMotionConstraint(commandToConvert.isHardConstraint()); if (!commandToConvert.isHardConstraint()) motionQPInputToPack.setWeight(commandToConvert.getWeight()); for (int jointIndex = 0; jointIndex < commandToConvert.getNumberOfJoints(); jointIndex++) InverseDynamicsJoint joint = commandToConvert.getJoint(jointIndex); int[] columns = jointIndexHandler.getJointIndices(joint); if (columns == null) motionQPInputToPack.taskJacobian.set(row, column, 1.0); CommonOps.insert(commandToConvert.getDesiredVelocity(jointIndex), motionQPInputToPack.taskObjective, row, 0); row += joint.getDegreesOfFreedom();
private void copyJointspaceVelocityCommand(JointspaceVelocityCommand commandToCopy) { JointspaceVelocityCommand localCommand = jointspaceVelocityCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }
public void retrieveJointsFromName(Map<String, OneDoFJointBasics> nameToJointMap) { for (int i = 0; i < jointspaceVelocityCommands.size(); i++) { JointspaceVelocityCommand command = jointspaceVelocityCommands.get(i); command.retrieveJointsFromName(nameToJointMap); } }
public void setDesiredAcceleration(int jointIndex, DenseMatrix64F desiredVelocity) { checkConsistency(joints.get(jointIndex), desiredVelocity); desiredVelocities.get(jointIndex).set(desiredVelocity); }
public JointspaceVelocityCommand() { clear(); }
public void retrieveJointsFromName(Map<String, ? extends InverseDynamicsJoint> nameToJointMap) { for (int i = 0; i < getNumberOfJoints(); i++) { joints.set(i, nameToJointMap.get(jointNames.get(i))); } }
private void copyJointspaceVelocityCommand(JointspaceVelocityCommand commandToCopy) { JointspaceVelocityCommand localCommand = jointspaceVelocityCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }
public void retrieveJointsFromName(Map<String, OneDoFJoint> nameToJointMap) { for (int i = 0; i < jointspaceVelocityCommands.size(); i++) { JointspaceVelocityCommand command = jointspaceVelocityCommands.get(i); command.retrieveJointsFromName(nameToJointMap); } }
public void addJoint(InverseDynamicsJoint joint, DenseMatrix64F desiredVelocity) { checkConsistency(joint, desiredVelocity); joints.add(joint); jointNames.add(joint.getName()); desiredVelocities.add().set(desiredVelocity); }