public static int getNumberOfVariables(JointType type) { return createJointState(null, type).getNumberOfStateVariables(); }
public static JointState createJointState(String name, JointType type) { switch (type) { case OneDoFJoint: return new OneDoFState(name); case SiXDoFJoint: return new SixDoFState(name); default: throw new RuntimeException("Unknown joint type" + type); } }
public static int getNumberOfJointStates(List<JointHolder> jointHolders) { int numberOfJointStates = 0; for (int i = 0; i < jointHolders.size(); i++) { numberOfJointStates += jointHolders.get(i).getNumberOfStateVariables(); } return numberOfJointStates; }
@Override public void update() { jointState.getRotation(rotationMatrix); jointState.getTranslation(tempVector); joint.setRotation(rotationMatrix); joint.setPosition(tempVector); jointState.getTwistAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); jointState.getTwistLinearPart(tempVector); joint.setVelocity(tempVector); } }
public static JointHolder getJointHolder(JointBasics joint) { if(joint instanceof SixDoFJoint) { return new SiXDoFJointHolder((SixDoFJoint) joint); } else if(joint instanceof OneDoFJointBasics) { return new OneDoFJointHolder((OneDoFJointBasics) joint); } else { throw new RuntimeException("Joint type " + joint.getClass().getSimpleName() + " not supported for communication"); } } }
public void update(long timestamp, long uid) { super.update(timestamp); this.uid = uid; int offset = 0; for (int i = 0; i < jointHolders.size(); i++) { JointHolder jointHolder = jointHolders.get(i); jointHolder.get(jointStates, offset); offset += jointHolder.getNumberOfStateVariables(); } }
private void addJointHolders(List<RigidBody> rootBodies) { InverseDynamicsJoint[] joints = ScrewTools.computeSubtreeJoints(rootBodies); for (InverseDynamicsJoint joint : joints) { JointHolder jointHolder = JointHolderFactory.getJointHolder(joint); YoProtoHandshake.JointDefinition.Builder jointDefinition = YoProtoHandshake.JointDefinition.newBuilder(); jointDefinition.setName(joint.getName()); jointDefinition.setType(jointHolder.getJointType()); yoProtoHandshakeBuilder.addJoint(jointDefinition); jointHolders.add(jointHolder); } }
private int getNumberOfJointStateVariables(YoProtoHandshake yoProtoHandshake) { int numberOfJointStates = 0; for (int i = 0; i < yoProtoHandshake.getJointCount(); i++) { JointDefinition joint = yoProtoHandshake.getJoint(i); numberOfJointStates += JointState.getNumberOfVariables(joint.getType()); } return numberOfJointStates; }
public static int getNumberOfJointStates(List<JointState> jointStates) { int numberOfJointStates = 0; for(int i = 0; i < jointStates.size(); i++) { numberOfJointStates += jointStates.get(i).getNumberOfStateVariables(); } return numberOfJointStates; } }
private void addJointStates(YoProtoHandshake yoProtoHandshake) { for (int i = 0; i < yoProtoHandshake.getJointCount(); i++) { JointDefinition joint = yoProtoHandshake.getJoint(i); jointStates.add(JointState.createJointState(joint.getName(), joint.getType())); } }
@Override public void update() { jointState.getRotation(rotationMatrix); jointState.getTranslation(tempVector); joint.setRotation(rotationMatrix); joint.setPosition(tempVector); jointState.getTwistAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); jointState.getTwistLinearPart(tempVector); joint.setVelocity(tempVector); } }
public static JointState createJointState(String name, JointType type) { switch (type) { case OneDoFJoint: return new OneDoFState(name); case SiXDoFJoint: return new SixDoFState(name); default: throw new RuntimeException("Unknown joint type" + type); } }
public static JointHolder getJointHolder(InverseDynamicsJoint joint) { if(joint instanceof SixDoFJoint) { return new SiXDoFJointHolder((SixDoFJoint) joint); } else if(joint instanceof OneDoFJoint) { return new OneDoFJointHolder((OneDoFJoint) joint); } else { throw new RuntimeException("Joint type " + joint.getClass().getSimpleName() + " not supported for communication"); } } }
private int getNumberOfJointStateVariables(YoProtoHandshake yoProtoHandshake) { int numberOfJointStates = 0; for (int i = 0; i < yoProtoHandshake.getJointCount(); i++) { JointDefinition joint = yoProtoHandshake.getJoint(i); numberOfJointStates += JointState.getNumberOfVariables(convertJointType(joint.getType())); } return numberOfJointStates; }
public static int getNumberOfJointStates(List<JointHolder> jointHolders) { int numberOfJointStates = 0; for (int i = 0; i < jointHolders.size(); i++) { numberOfJointStates += jointHolders.get(i).getNumberOfStateVariables(); } return numberOfJointStates; }
public static int getNumberOfJointStates(List<JointState> jointStates) { int numberOfJointStates = 0; for(int i = 0; i < jointStates.size(); i++) { numberOfJointStates += jointStates.get(i).getNumberOfStateVariables(); } return numberOfJointStates; } }
private int getNumberOfJointStateVariables(Handshake handshake) { int numberOfJointStates = 0; for (int i = 0; i < handshake.getJoints().size(); i++) { JointDefinition joint = handshake.getJoints().get(i); numberOfJointStates += JointState.getNumberOfVariables(joint.getType()); } return numberOfJointStates; }
public static int getNumberOfVariables(Handshake handShake) { int jointStateVariables = 0; for (int i = 0; i < handShake.getJoints().size(); i++) { jointStateVariables += JointState.getNumberOfVariables(handShake.getJoints().get(i).getType()); } return 1 + handShake.getVariables().size() + jointStateVariables; }