private Matrix3d createInertiaMatrixCylinder(LinkNames linkName) {
Matrix3d inertiaCylinder = new Matrix3d();
inertiaCylinder =
RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidCylinder(
RobotParameters.MASSES.get(linkName),
RobotParameters.RADII.get(linkName),
RobotParameters.LENGTHS.get(linkName),
Axis.Z);
return inertiaCylinder;
}
/** *********************** SCS ROBOT - Links ******************************* */
private void createAndAttachBodyLink(LinkNames linkName) {
Link link = new Link(LinkNames.BODY_LINK.getName());
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaBody);
link.setMass(RobotParameters.MASSES.get(LinkNames.BODY_LINK));
link.setComOffset(comOffsetBody);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCube(cubeX, cubeY, cubeZ, RobotParameters.APPEARANCE.get(LinkNames.BODY_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.7);
bodyJointSCS.setLink(link);
}
private void createAndAttachFootLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(LinkNames.FOOT_LINK.getName());
Matrix3d inertiaFoot = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaFoot);
link.setMass(RobotParameters.MASSES.get(LinkNames.FOOT_LINK));
link.setComOffset(comOffsetFoot);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.translate(footOffsetX, 0.0, 0.0);
linkGraphics.addCube(footX, footY, -footZ, RobotParameters.APPEARANCE.get(LinkNames.FOOT_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
private void createAndAttachCylinderLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(linkName.getName());
Matrix3d inertiaCylinder = createInertiaMatrixCylinder(linkName);
link.setMomentOfInertia(inertiaCylinder);
link.setMass(RobotParameters.MASSES.get(linkName));
link.setComOffset(new Vector3d(0.0, 0.0, -RobotParameters.LENGTHS.get(linkName) / 2.0));
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCylinder(
-RobotParameters.LENGTHS.get(linkName),
RobotParameters.RADII.get(linkName),
RobotParameters.APPEARANCE.get(linkName));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
public class Step6IDandSCSRobot_pinKnee extends Robot {
/** Variables */
// ID
private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
private final ReferenceFrame elevatorFrame =
ReferenceFrame.constructFrameWithUnchangingTransformToParent(
"elevator", worldFrame, new RigidBodyTransform());
private FramePoint bodyPosition = new FramePoint();
private final Vector3d jointAxesPinJoints =
new Vector3d(0.0, 1.0, 0.0); // rotate around Y-axis (for revolute joints)
private final RigidBody elevator;
private final SideDependentList<EnumMap<JointNames, OneDoFJoint>> allLegJoints =
SideDependentList.createListOfEnumMaps(JointNames.class); // Includes the ankle!
private final LinkedHashMap<OneDoFJoint, OneDegreeOfFreedomJoint> idToSCSLegJointMap =
new LinkedHashMap<
OneDoFJoint,
OneDegreeOfFreedomJoint>(); // makes the SCS joints available without another side
// dependent enum
private final SideDependentList<EnumMap<LinkNames, RigidBody>> allLegRigidBodies =
SideDependentList.createListOfEnumMaps(LinkNames.class);
private final SixDoFJoint bodyJointID;
// SCS
private final FloatingPlanarJoint bodyJointSCS;
SideDependentList<GroundContactPoint> GCpointsHeel = new SideDependentList<GroundContactPoint>();
SideDependentList<GroundContactPoint> GCpointsToe = new SideDependentList<GroundContactPoint>();
private double cubeX = RobotParameters.BODY_DIMENSIONS.get(Axis.X);
private double cubeY = RobotParameters.BODY_DIMENSIONS.get(Axis.Y);
private double cubeZ = RobotParameters.BODY_DIMENSIONS.get(Axis.Z);
private double footX = RobotParameters.FOOT_DIMENSIONS.get(Axis.X);
private double footY = RobotParameters.FOOT_DIMENSIONS.get(Axis.Y);
private double footZ = RobotParameters.FOOT_DIMENSIONS.get(Axis.Z);
// GENERAL
private double bodyMass = RobotParameters.MASSES.get(LinkNames.BODY_LINK);
private double footMass = RobotParameters.MASSES.get(LinkNames.FOOT_LINK);
private double hipOffsetY = RobotParameters.BODY_DIMENSIONS.get(Axis.X) / 2.0;
private double kneeOffsetZ = -RobotParameters.LENGTHS.get(LinkNames.UPPER_LINK) + 0.1;
private double ankleOffsetZ = -RobotParameters.LENGTHS.get(LinkNames.LOWER_LINK);
private double footOffsetX = 0.15;
private double gcOffset = -footZ;
private double gcRadius = 0.03;
Vector3d comOffsetBody = new Vector3d(0.0, 0.0, cubeZ / 2.0);
Vector3d comOffsetFoot = new Vector3d(footX / 2.0 - 0.075, 0.0, -footZ / 2.0);
private double bodyZ, bodyX; // global so that it is created only once (avoid generating garbage)
public DoubleYoVariable qd_x;
private double initialBodyHeight =
RobotParameters.LENGTHS.get(LinkNames.UPPER_LINK)
+ RobotParameters.LENGTHS.get(LinkNames.LOWER_LINK)
- 0.1
+ footZ
- 0.029
+ 0.024;
/** Joints */
public Step6IDandSCSRobot_pinKnee() {
super("Robot");
/** **************** ID ROBOT ********************** */
elevator = new RigidBody("elevator", elevatorFrame);
bodyJointID = new SixDoFJoint(JointNames.BODY.getName(), elevator, elevatorFrame);
createAndAttachBodyRB(LinkNames.BODY_LINK, bodyJointID);
for (RobotSide robotSide : RobotSide.values) {
// HIP ID (location, joint, and rigidBody)
Vector3d hipOffset = new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0);
RevoluteJoint hipJointID =
ScrewTools.addRevoluteJoint(
JointNames.HIP.getName(),
bodyJointID.getSuccessor(),
hipOffset,
jointAxesPinJoints); // The parent rigid body of the hip joint is:
// bodyJointID.getSuccessor()
allLegJoints.get(robotSide).put(JointNames.HIP, hipJointID);
createAndAttachCylinderRB(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);
// KNEE ID
Vector3d kneeOffset = new Vector3d(0.0, 0.0, kneeOffsetZ);
RevoluteJoint kneeJointID =
ScrewTools.addRevoluteJoint(
JointNames.KNEE.getName(), hipJointID.getSuccessor(), kneeOffset, jointAxesPinJoints);
allLegJoints.get(robotSide).put(JointNames.KNEE, kneeJointID);
createAndAttachCylinderRB(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);
// ANKLE ID (location, joint, and rigidBody)
Vector3d ankleOffset = new Vector3d(0.0, 0.0, ankleOffsetZ);
RevoluteJoint ankleJointID =
ScrewTools.addRevoluteJoint(
JointNames.ANKLE.getName(),
kneeJointID.getSuccessor(),
ankleOffset,
jointAxesPinJoints);
allLegJoints.get(robotSide).put(JointNames.ANKLE, ankleJointID);
createAndAttachFootRB(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
}
/** **************** SCS ROBOT ********************** */
// BODY SCS
bodyJointSCS = new FloatingPlanarJoint(JointNames.BODY.getName(), this);
this.addRootJoint(bodyJointSCS);
createAndAttachBodyLink(LinkNames.BODY_LINK);
bodyJointSCS.setCartesianPosition(0.0, initialBodyHeight);
for (RobotSide robotSide : RobotSide.values) {
// HIP SCS
PinJoint hipJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.HIP.getName(),
new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0),
this,
jointAxesPinJoints);
hipJointSCS.setLimitStops(-0.75, 0.75, 1e3, 1e1);
// It is NOT necessary to set limits in the ID description because if the SCS description
// doesn't let the robot move passed a point the ID robot won't be able to pass it either
if (robotSide == RobotSide.LEFT) {
hipJointSCS.setQ(-0.6);
} else {
hipJointSCS.setQ(0.1);
}
bodyJointSCS.addJoint(hipJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.HIP), hipJointSCS);
createAndAttachCylinderLink(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);
// KNEE SCS
PinJoint kneeJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.KNEE.getName(),
new Vector3d(0.0, 0.0, kneeOffsetZ),
this,
jointAxesPinJoints);
kneeJointSCS.setLimitStops(-0.01, 1.8, 1e5, 1e3);
hipJointSCS.addJoint(kneeJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.KNEE), kneeJointSCS);
createAndAttachCylinderLink(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);
// ANKLE SCS
PinJoint ankleJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.ANKLE.getName(),
new Vector3d(0.0, 0.0, ankleOffsetZ),
this,
jointAxesPinJoints);
ankleJointSCS.setLimitStops(-0.7, 0.7, 1e3, 1e2);
if (robotSide == RobotSide.RIGHT) {
ankleJointSCS.setQ(-0.1);
}
kneeJointSCS.addJoint(ankleJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.ANKLE), ankleJointSCS);
// FEET SCS
createAndAttachFootLink(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
GroundContactPoint gcHeel =
new GroundContactPoint(
robotSide.getSideNameFirstLetter() + "gcHeel",
new Vector3d(-0.1, 0.0, gcOffset),
this);
GCpointsHeel.set(robotSide, gcHeel);
ankleJointSCS.addGroundContactPoint(gcHeel);
Graphics3DObject graphicsGCheel = ankleJointSCS.getLink().getLinkGraphics();
graphicsGCheel.identity();
graphicsGCheel.translate(-0.1, 0.0, gcOffset);
graphicsGCheel.addSphere(gcRadius, YoAppearance.Orange());
if (robotSide == RobotSide.RIGHT) {
setFStoTrue(robotSide);
}
GroundContactPoint gcToe =
new GroundContactPoint(
robotSide.getSideNameFirstLetter() + "gcToe", new Vector3d(0.4, 0.0, gcOffset), this);
GCpointsToe.set(robotSide, gcToe);
ankleJointSCS.addGroundContactPoint(gcToe);
Graphics3DObject graphics = ankleJointSCS.getLink().getLinkGraphics();
graphics.identity();
graphics.translate(0.4, 0.0, gcOffset);
graphics.addSphere(gcRadius, YoAppearance.DarkOliveGreen());
}
/** ************** SCS Ground Model ************************ */
GroundContactModel groundModel =
new LinearGroundContactModel(this, 1e3, 1e3, 5e3, 1e3, this.getRobotsYoVariableRegistry());
GroundProfile3D profile = new FlatGroundProfile();
groundModel.setGroundProfile3D(profile);
this.setGroundContactModel(groundModel);
}
/** Initialization for walking */
public void setInitialVelocity() {
qd_x = (DoubleYoVariable) getVariable("qd_x");
qd_x.set(0.8);
}
/** Inertias */
private Matrix3d createInertiaMatrixBox(LinkNames linkName) {
Matrix3d inertiaBox = new Matrix3d();
if (linkName == LinkNames.BODY_LINK) {
inertiaBox =
RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidBox(
cubeX, cubeY, cubeZ, bodyMass);
} else {
inertiaBox =
RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidBox(
footX, footY, footZ, footMass);
}
return inertiaBox;
}
private Matrix3d createInertiaMatrixCylinder(LinkNames linkName) {
Matrix3d inertiaCylinder = new Matrix3d();
inertiaCylinder =
RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidCylinder(
RobotParameters.MASSES.get(linkName),
RobotParameters.RADII.get(linkName),
RobotParameters.LENGTHS.get(linkName),
Axis.Z);
return inertiaCylinder;
}
/** Rigid Bodies and Links */
/** *********************** ID ROBOT - Rigid bodies ******************************* */
private void createAndAttachBodyRB(LinkNames linkName, SixDoFJoint bodyJointID) {
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
ScrewTools.addRigidBody(linkName.getName(), bodyJointID, inertiaBody, bodyMass, comOffsetBody);
}
private void createAndAttachCylinderRB(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Matrix3d inertiaCylinder = createInertiaMatrixCylinder(linkName);
Vector3d comOffsetCylinder =
new Vector3d(0.0, 0.0, -RobotParameters.LENGTHS.get(linkName) / 2.0);
allLegRigidBodies
.get(robotSide)
.put(
linkName,
ScrewTools.addRigidBody(
linkName.getName(),
allLegJoints.get(robotSide).get(jointName),
inertiaCylinder,
bodyMass,
comOffsetCylinder));
}
private void createAndAttachFootRB(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
ScrewTools.addRigidBody(
linkName.getName(),
allLegJoints.get(robotSide).get(JointNames.KNEE),
inertiaBody,
footMass,
comOffsetFoot);
}
/** *********************** SCS ROBOT - Links ******************************* */
private void createAndAttachBodyLink(LinkNames linkName) {
Link link = new Link(LinkNames.BODY_LINK.getName());
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaBody);
link.setMass(RobotParameters.MASSES.get(LinkNames.BODY_LINK));
link.setComOffset(comOffsetBody);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCube(cubeX, cubeY, cubeZ, RobotParameters.APPEARANCE.get(LinkNames.BODY_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.7);
bodyJointSCS.setLink(link);
}
private void createAndAttachCylinderLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(linkName.getName());
Matrix3d inertiaCylinder = createInertiaMatrixCylinder(linkName);
link.setMomentOfInertia(inertiaCylinder);
link.setMass(RobotParameters.MASSES.get(linkName));
link.setComOffset(new Vector3d(0.0, 0.0, -RobotParameters.LENGTHS.get(linkName) / 2.0));
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCylinder(
-RobotParameters.LENGTHS.get(linkName),
RobotParameters.RADII.get(linkName),
RobotParameters.APPEARANCE.get(linkName));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
private void createAndAttachFootLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(LinkNames.FOOT_LINK.getName());
Matrix3d inertiaFoot = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaFoot);
link.setMass(RobotParameters.MASSES.get(LinkNames.FOOT_LINK));
link.setComOffset(comOffsetFoot);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.translate(footOffsetX, 0.0, 0.0);
linkGraphics.addCube(footX, footY, -footZ, RobotParameters.APPEARANCE.get(LinkNames.FOOT_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
/** SCS Robot --> ID Robot Send positions and velocities. */
public void updatePositionsIDrobot() {
// Body info
bodyJointID.setPosition(
bodyJointSCS.getQ_t1().getDoubleValue(), 0.0, bodyJointSCS.getQ_t2().getDoubleValue());
bodyJointID.setRotation(0.0, bodyJointSCS.getQ_rot().getDoubleValue(), 0.0);
double[] velocityArray = new double[6];
velocityArray[0] = 0.0; // yaw
velocityArray[1] = bodyJointSCS.getQd_rot().getDoubleValue(); // pitch
velocityArray[2] = 0.0; // roll
velocityArray[3] = bodyJointSCS.getQd_t1().getDoubleValue(); // x
velocityArray[4] = 0.0; // y
velocityArray[5] = bodyJointSCS.getQd_t2().getDoubleValue(); // z
DenseMatrix64F velocities = new DenseMatrix64F(6, 1, true, velocityArray);
bodyJointID.setVelocity(velocities, 0);
// Leg and foot info (hip, knee and ankle)
for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
idJoint.setQ(scsJoint.getQYoVariable().getDoubleValue());
idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue());
}
elevator.updateFramesRecursively();
// Get the body coordinates
bodyPosition = new FramePoint();
bodyPosition.setToZero(bodyJointID.getFrameAfterJoint());
bodyPosition.changeFrame(worldFrame);
}
/** ID Robot --> SCS Robot Copy the torques from the IDRobot to the SCSRobot. */
public void updateTorquesSCSrobot() // Remember! the body joint is NOT actuated
{
for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
scsJoint.setTau(idJoint.getTau());
}
}
public RigidBody getElevator() {
return elevator;
}
/** Getters and Setters for the controller */
// ID joints and rigid bodies
public OneDoFJoint getLegJoint(JointNames jointName, RobotSide robotSide) {
return allLegJoints.get(robotSide).get(jointName);
}
public SixDoFJoint getBodyJoint() {
return bodyJointID;
}
public RigidBody getLegRigidBody(LinkNames linkName, RobotSide robotSide) {
return allLegRigidBodies.get(robotSide).get(linkName);
}
// Body
public double getBodyPositionZ() {
bodyZ = bodyPosition.getZ();
return bodyZ;
}
public double getBodyPositionX() {
bodyX = bodyPosition.getX();
return bodyX;
}
// public void getBodyPoint(Point3d bodyPointToPack) // Example of GET TO PACK method
// {
// Point3d bodyPoint = this.bodyPosition.getPoint();
// bodyPointToPack.set(bodyPoint);
// }
public void getBodyPitch(Quat4d rotationToPack) {
bodyJointID.getFrameAfterJoint().getRotation(rotationToPack);
}
public void getBodyLinearVel(Vector3d linearVelocityToPack) {
bodyJointID.getLinearVelocity(linearVelocityToPack);
}
public void getBodyAngularVel(Vector3d angularVelocityToPack) {
bodyJointID.getAngularVelocity(angularVelocityToPack);
}
public double getBodyVelX(Vector3d linearVelocityToPack) {
bodyJointID.getLinearVelocity(linearVelocityToPack);
double velX = linearVelocityToPack.getX();
return velX;
}
// Knee
public double getKneeVelocity(RobotSide robotSide) {
double kneeVelocity = allLegJoints.get(robotSide).get(JointNames.KNEE).getQd();
return kneeVelocity;
}
public void setKneeTau(RobotSide robotSide, double desiredKneeTau) {
allLegJoints.get(robotSide).get(JointNames.KNEE).setTau(desiredKneeTau);
}
public double getKneePosition(RobotSide robotSide) {
double kneeHeight = allLegJoints.get(robotSide).get(JointNames.KNEE).getQ();
return kneeHeight;
}
// Hip
public void setHipTau(RobotSide robotSide, double desiredHipTau) {
allLegJoints.get(robotSide).get(JointNames.HIP).setTau(desiredHipTau);
}
public double getHipVelocity(RobotSide robotSide) {
double hipVelocity = allLegJoints.get(robotSide).get(JointNames.HIP).getQd();
return hipVelocity;
}
public double getHipPitch(RobotSide robotSide) {
double hipPitch = allLegJoints.get(robotSide).get(JointNames.HIP).getQ();
return hipPitch;
}
// Ankle
public void setAnkleTau(RobotSide robotSide, double desiredAnkleTau) {
allLegJoints.get(robotSide).get(JointNames.ANKLE).setTau(desiredAnkleTau);
}
public double getAnkleVelocity(RobotSide robotSide) {
double ankleVelocity = allLegJoints.get(robotSide).get(JointNames.ANKLE).getQd();
return ankleVelocity;
}
public double getAnklePitch(RobotSide robotSide) {
double anklePitch = allLegJoints.get(robotSide).get(JointNames.ANKLE).getQ();
return anklePitch;
}
// Feet
public boolean heelOnTheFloor(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).isInContact();
}
public boolean heelToeOffAhead(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getX()
> GCpointsHeel.get(robotSide.getOppositeSide()).getX();
}
public boolean toeOnTheFloor(RobotSide robotSide) {
return GCpointsToe.get(robotSide).isInContact();
}
public void setFStoTrue(RobotSide robotSide) {
GCpointsHeel.get(robotSide).getYoFootSwitch().set(1.0);
}
public double getToeX(RobotSide robotSide) {
return GCpointsToe.get(robotSide).getX();
}
public double getHeelX(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getX();
}
public double getHeelZ(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getZ();
}
}
