Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -228,11 +228,11 @@ private TransformNR updateLines(CartesianManipulator m, CartesianManipulator p,
double xyRot = Math.toDegrees(Math.atan2(cp1Ydiff, cp1XDiff)) - 90;

TransformNR az = new TransformNR(0, 0, 0, new RotationNR(0, -xyRot, 0));
TransformNR reorented = az.times(vect);
// com.neuronrobotics.sdk.common.Log.error("CP1 "+reorented.getX()+"
// "+reorented.getY()+"
// "+reorented.getZ());
double xzRot = Math.toDegrees(Math.atan2(reorented.getZ(), reorented.getY()));
TransformNR reoriented = az.times(vect);
// com.neuronrobotics.sdk.common.Log.error("CP1 "+reoriented.getX()+"
// "+reoriented.getY()+"
// "+reoriented.getZ());
double xzRot = Math.toDegrees(Math.atan2(reoriented.getZ(), reoriented.getY()));

TransformNR pose = new TransformNR(0, 0, 0, new RotationNR(xzRot, xyRot, 0));

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,8 @@ public void update(float timeStep) {
TransformFactory.nrToBullet(new TransformNR(gravity.x, gravity.y, gravity.z, new RotationNR()),
gravTrans);
TransformFactory.nrToBullet(
new TransformNR(0, 0, 0, orientation.w, orientation.x, orientation.y, orientation.z), orientTrans);
new TransformNR(0, 0, 0, orientation.w, orientation.x, orientation.y, orientation.z),
orientTrans);
orientTrans.inverse();
orientTrans.mul(gravTrans);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -154,9 +154,9 @@ public void controlStep(MuJoCoModelManager manager) {
String s = iterator.next();
if (bodyName.contentEquals(s)) {
TransformNR tf = mujocoToTransformNR(manager.getBodyPose(s));
Double xAcceleration = Math.toDegrees(tf.getRotation().getRotationTilt());
Double yAcceleration = Math.toDegrees(tf.getRotation().getRotationAzimuth());
Double zAcceleration = Math.toDegrees(tf.getRotation().getRotationElevation());
Double xAcceleration = Math.toDegrees(tf.getRotation().getRotationTiltRadians());
Double yAcceleration = Math.toDegrees(tf.getRotation().getRotationAzimuthRadians());
Double zAcceleration = Math.toDegrees(tf.getRotation().getRotationElevationRadians());

Double rotxAcceleration = 0.0;
Double rotyAcceleration = 0.0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -182,9 +182,9 @@ public static eu.mihosoft.vrl.v3d.Transform nrToCSG(TransformNR nr) {

public static TransformNR scale(TransformNR incoming, double scale) {
return new TransformNR(incoming.getX() * scale, incoming.getY() * scale, incoming.getZ() * scale,
new RotationNR(Math.toDegrees(incoming.getRotation().getRotationTilt()) * scale,
Math.toDegrees(incoming.getRotation().getRotationAzimuth()) * scale,
Math.toDegrees(incoming.getRotation().getRotationElevation()) * scale));
new RotationNR(Math.toDegrees(incoming.getRotation().getRotationTiltRadians()) * scale,
Math.toDegrees(incoming.getRotation().getRotationAzimuthRadians()) * scale,
Math.toDegrees(incoming.getRotation().getRotationElevationRadians()) * scale));
}

public static TransformNR csgToNR(eu.mihosoft.vrl.v3d.Transform csg) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -153,8 +153,9 @@ public static void addSVGToFreeCAD(File freecadModel, File SVG, Transform pose,
args.add(freecadModel.getAbsolutePath());
args.add(SVG.getAbsolutePath());
args.add("\"" + nr.getX() + "," + nr.getY() + "," + nr.getZ() + "\"");
args.add("\"" + Math.toDegrees(r.getRotationAzimuth()) + "," + Math.toDegrees(r.getRotationElevation())
+ "," + Math.toDegrees(r.getRotationTilt()) + "\"");
args.add("\"" + Math.toDegrees(r.getRotationAzimuthRadians()) + ","
+ Math.toDegrees(r.getRotationElevationRadians()) + "," + Math.toDegrees(r.getRotationTiltRadians())
+ "\"");
args.add(name);
args.add(bodyName);
legacySystemRun(null, export.getAbsoluteFile().getParentFile(), System.out, args);
Expand Down
Loading