Merge pull request #66 from dojo-sim/jan/fix_quaternion_deprecation

Deprecated Quaternions functions are removed, but Quaternions still calls them internally which causes depwarns slowing down and breaking tests. Should be ready for Quaternions 0.7.0.
Limit compat to Quaternions 0.5.7 for now, skip 0.6.0, and try again with 0.7.0.

DojoEnvironments/src/panda/methods/dev.jl

@@ -14,7 +14,7 @@ mech = DojoEnvironments.get_mechanism(:panda,
     limits=true,
     )
 # for joint in mech.joints
-#     joint.rotational.orientation_offset = Quaternion(1,0,0,0.0,true)
+#     joint.rotational.orientation_offset = Quaternion(1,0,0,0.0)
 # end
 function ctrl!(m, k; kp=10.0, kd=5.0)
     nu = input_dimension(m)
@@ -119,7 +119,7 @@ storage.ω[1]
 plot([-ω[1] for ω in storage.ω[1]])
 
 xa = szeros(3)
-qa = Quaternions.QuaternionF64(1.0, 0.0, 0.0, 0.0, true)
+qa = Quaternions.QuaternionF64(1.0, 0.0, 0.0, 0.0)
 xb = 0*[0.0, 0.4476047997637539, 0.8771773278369399]
 qb = mech.bodies[1].state.q2
 Q = displacement_jacobian_configuration(:child, mech.joints[1].rotational,
@@ -154,31 +154,31 @@ p0 = rand(3)
 J0 = Dojo.impulse_transform_jacobian(:parent, :parent, rot0, xa, qa, xb, qb, p0)
 attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
 J1 = FiniteDiff.finite_difference_jacobian(
-    z -> Dojo.impulse_transform(:parent, rot0, z[1:3], Quaternion(z[4:7]...,true), xb, qb) * p0,
+    z -> Dojo.impulse_transform(:parent, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0,
     [xa; Dojo.vector(qa)]
     ) * attjac
 @test norm(J0 - J1, Inf) < 1.0e-7
 
 J0 = Dojo.impulse_transform_jacobian(:parent, :child, rot0, xa, qa, xb, qb, p0)
 attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2))
 J1 = FiniteDiff.finite_difference_jacobian(
-    z -> Dojo.impulse_transform(:parent, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...,true)) * p0,
+    z -> Dojo.impulse_transform(:parent, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0,
     [xb; Dojo.vector(qb)]
     ) * attjac
 @test norm(J0 - J1, Inf) < 1.0e-7
 
 J0 = Dojo.impulse_transform_jacobian(:child, :parent, rot0, xa, qa, xb, qb, p0)
 attjac = cat(I(3),Dojo.LVᵀmat(qa), dims=(1,2))
 J1 = FiniteDiff.finite_difference_jacobian(
-    z -> Dojo.impulse_transform(:child, rot0, z[1:3], Quaternion(z[4:7]...,true), xb, qb) * p0,
+    z -> Dojo.impulse_transform(:child, rot0, z[1:3], Quaternion(z[4:7]...), xb, qb) * p0,
     [xa; Dojo.vector(qa)]
     ) * attjac
 @test norm(J0 - J1, Inf) < 1.0e-7
 
 J0 = Dojo.impulse_transform_jacobian(:child, :child, rot0, xa, qa, xb, qb, p0)
 attjac = cat(I(3),Dojo.LVᵀmat(qb), dims=(1,2))
 J1 = FiniteDiff.finite_difference_jacobian(
-    z -> Dojo.impulse_transform(:child, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...,true)) * p0,
+    z -> Dojo.impulse_transform(:child, rot0, xa, qa, z[1:3], Quaternion(z[4:7]...)) * p0,
     [xb; Dojo.vector(qb)]
     ) * attjac
 @test norm(J0 - J1, Inf) < 1.0e-7
@@ -191,14 +191,14 @@ p0 = rand(3)
 q0 = rand(QuatRotation).q
 J0 = Dojo.∂rotation_matrix∂q(q0, p0)
 J2 = Dojo.∂rotation_matrix∂q(q0, p0, attjac=true)
-J1 = FiniteDiff.finite_difference_jacobian(q0 -> Dojo.rotation_matrix(Quaternion(q0...,true))*p0, Dojo.vector(q0))
+J1 = FiniteDiff.finite_difference_jacobian(q0 -> Dojo.rotation_matrix(Quaternion(q0...))*p0, Dojo.vector(q0))
 J3 = J1 * Dojo.LVᵀmat(q0)
 norm(J0 - J1, Inf)
 norm(J2 - J3, Inf)
 
 J4 = Dojo.∂rotation_matrix_inv∂q(q0, p0)
 J6 = Dojo.∂rotation_matrix_inv∂q(q0, p0, attjac=true)
-J5 = FiniteDiff.finite_difference_jacobian(q0 -> Dojo.rotation_matrix(inv(Quaternion(q0...,true)))*p0, Dojo.vector(q0))
+J5 = FiniteDiff.finite_difference_jacobian(q0 -> Dojo.rotation_matrix(inv(Quaternion(q0...)))*p0, Dojo.vector(q0))
 J7 = J5 * Dojo.LVᵀmat(q0)
 norm(J4 - J5, Inf)
 norm(J6 - J7, Inf)
@@ -226,7 +226,7 @@ VRᵀmat(qra) * LVᵀmat(qra) * x - vector_rotate(x, qra)
 VLmat(qra) * RᵀVᵀmat(qra) * x - vector_rotate(x, qra)
 
 qra = [1,0,0,1.]
-qra = Quaternion(qra ./ norm(qra)..., true)
+qra = Quaternion(qra ./ norm(qra)...)
 x = [1,0,0]
 vector_rotate(x, qra)
 VLmat(qra) * RᵀVᵀmat(qra) * x
@@ -282,7 +282,7 @@ mech = DojoEnvironments.get_mechanism(:panda,
     gravity=[0,0,-9.0],
     spring=0.0,
     damper=0.1)
-# mech.joints[1].rotational.orientation_offset = Quaternion(1,0,0,0.0,true)
+# mech.joints[1].rotational.orientation_offset = Quaternion(1,0,0,0.0)
 function ctrl!(m,k)
     nu = minimal_dimension(mech)
     set_input!(m, 0.000 * 1 *sones(nu))
@@ -319,7 +319,7 @@ damper_force(:child, joint.rotational, current_configuration(pbody.state)[2], pb
 function min_vel(qb)
     xa = SVector{3}([0.0, 0.0, 0.0])
     va = SVector{3}([0.0, 0.0, 0.0])
-    qa = Quaternions.QuaternionF64(1.0, 0.0, 0.0, 0.0, true)
+    qa = Quaternions.QuaternionF64(1.0, 0.0, 0.0, 0.0)
     ωa = SVector{3}([0.0, 0.0, 0.0])
     xb = SVector{3}([0.0, 0.0, 0.0])
     vb = SVector{3}([0.0, 0.0, 0.0])

DojoEnvironments/src/rexhopper/methods/demo.jl

@@ -12,7 +12,7 @@ mech = get_rexhopper(timestep=0.01, gravity= -0.99 * 9.81, model="rexhopper_no_w
     floating=true, contact_foot=true, limits=true, spring=0.0, damper=0.5, contact_type=:linear)
 
 q0 = [1,0.5,0,0]
-q0 = Quaternion(q0 ./ norm(q0)..., true)
+q0 = Quaternion(q0 ./ norm(q0)...)
 initialize!(mech, :rexhopper, body_position=[0,0,0.4], body_orientation=[0.5,0.8,0.0])
 z0 = get_maximal_state(mech)
 

Project.toml

@@ -43,7 +43,7 @@ MeshCat = "<0.14.2, 0.14.2"
 Meshing = "<0.6.0, 0.6.0"
 Parameters = "<0.12.3, 0.12.3"
 Polyhedra = "<0.7.5, 0.7.5"
-Quaternions = "0.5.2, 0.6.0, 0.6.1"
+Quaternions = "0.5.2, 0.5.7"
 Scratch = "1.1, 1.1"
 StaticArrays = "1.2, 1.5"
 julia = "1.6, 1.8"

examples/simulation/dev/capsule_capsule_v3.jl

@@ -294,8 +294,8 @@ setobject!(vis[:cap2][:tip], sph2, MeshPhongMaterial(color=color2))
 settransform!(vis[:cap2][:tip], Translation(0.0, 0.0, 0.5 * hb))
 
 # set configuration
-settransform!(vis[:cap1], compose(Translation(xa), LinearMap(Quaternion(qa..., false))))
-settransform!(vis[:cap2], compose(Translation(xb), LinearMap(Quaternion(qb..., false))))
+settransform!(vis[:cap1], compose(Translation(xa), LinearMap(Quaternion(qa...)))
+settransform!(vis[:cap2], compose(Translation(xb), LinearMap(Quaternion(qb...)))
 
 # closest points
 color_cp = Colors.RGBA(0.0, 0.0, 0.0, 1.0);

examples/simulation/dev/capsule_capsule_v4.jl

@@ -259,8 +259,8 @@ setobject!(vis[:cap2][:tip], sph2, MeshPhongMaterial(color=color2))
 settransform!(vis[:cap2][:tip], Translation(0.0, 0.0, 0.5 * hb))
 
 # set configuration
-settransform!(vis[:cap1], compose(Translation(xa), LinearMap(Quaternion(qa..., false))))
-settransform!(vis[:cap2], compose(Translation(xb), LinearMap(Quaternion(qb..., false))))
+settransform!(vis[:cap1], compose(Translation(xa), LinearMap(Quaternion(qa...))))
+settransform!(vis[:cap2], compose(Translation(xb), LinearMap(Quaternion(qb...))))
 
 # closest points
 color_cp = Colors.RGBA(0.0, 0.0, 0.0, 1.0);

examples/simulation/dev/collision_sphere_box.jl

@@ -72,7 +72,7 @@ mech = Mechanism(origin, bodies, joints, contacts,
 mech.bodies[1].state.x2 = [0.0, 0.0, 1.5]
 q = rand(4) 
 q ./= norm(q) 
-mech.bodies[1].state.q2 = Quaternion(q..., true)
+mech.bodies[1].state.q2 = Quaternion(q...)
 mech.bodies[2].state.x2 = [0.3, 0.25, 3.0]
 
 storage = simulate!(mech, 5.0, 

examples/simulation/dev/collision_sphere_capsule.jl

@@ -51,7 +51,7 @@ mech = Mechanism(origin, bodies, joints, contacts,
 mech.bodies[1].state.x2 = [0.25, 0.25, 1.5]
 q = rand(4)
 q ./= norm(q)
-mech.bodies[1].state.q2 = Quaternion(q..., true)
+mech.bodies[1].state.q2 = Quaternion(q...)
 mech.bodies[2].state.x2 = [0.0, 0.5, 4.0]
 
 storage = simulate!(mech, 2.5, 

examples/simulation/dev/constraint.jl

@@ -18,14 +18,14 @@ function sdf_capsule_capsule_constraint_jacobian(relative, xa, qa, ha, ra, xb, q
     if relative == :parent 
         dx = FiniteDiff.finite_difference_jacobian(x -> closest_points_capsule_capsule(x, qa, ha, ra, xb, qb, hb, rb)[2], xa)
         dx -= FiniteDiff.finite_difference_jacobian(x -> closest_points_capsule_capsule(x, qa, ha, ra, xb, qb, hb, rb)[1], xa)
-        dq = FiniteDiff.finite_difference_jacobian(q -> closest_points_capsule_capsule(xa, Quaternion(q..., false), ha, ra, xb, qb, hb, rb)[2], vector(qa))
-        dq -= FiniteDiff.finite_difference_jacobian(q -> closest_points_capsule_capsule(xa, Quaternion(q..., false), ha, ra, xb, qb, hb, rb)[1], vector(qa))
+        dq = FiniteDiff.finite_difference_jacobian(q -> closest_points_capsule_capsule(xa, Quaternion(q...), ha, ra, xb, qb, hb, rb)[2], vector(qa))
+        dq -= FiniteDiff.finite_difference_jacobian(q -> closest_points_capsule_capsule(xa, Quaternion(q...), ha, ra, xb, qb, hb, rb)[1], vector(qa))
         attjac && (Q *= LVᵀmat(qa))
     elseif relative == :child 
         dx = FiniteDiff.finite_difference_jacobian(x -> closest_points_capsule_capsule(xa, qa, ha, ra, x, qb, hb, rb)[2], xb)
         dx -= FiniteDiff.finite_difference_jacobian(x -> closest_points_capsule_capsule(xa, qa, ha, ra, x, qb, hb, rb)[1], xb)
-        dq = FiniteDiff.finite_difference_jacobian(q -> closest_points_capsule_capsule(xa, qa, ha, ra, xb, Quaternion(q..., false), hb, rb)[2], vector(qb))
-        dq -= FiniteDiff.finite_difference_jacobian(q -> closest_points_capsule_capsule(xa, qa, ha, ra, xb, Quaternion(q..., false), hb, rb)[1], vector(qb))
+        dq = FiniteDiff.finite_difference_jacobian(q -> closest_points_capsule_capsule(xa, qa, ha, ra, xb, Quaternion(q...), hb, rb)[2], vector(qb))
+        dq -= FiniteDiff.finite_difference_jacobian(q -> closest_points_capsule_capsule(xa, qa, ha, ra, xb, Quaternion(q...), hb, rb)[1], vector(qb))
         attjac && (Q *= LVᵀmat(qb))
     end
 
@@ -37,10 +37,10 @@ Jc = constraint_jacobian(:child, p1, o1, h1, r1, p2, o2, h2, r2, attjac=false)
 
 using FiniteDiff
 Jpx = FiniteDiff.finite_difference_jacobian(x -> constraint(x, o1, h1, r1, p2, o2, h2, r2), p1)
-Jpq = FiniteDiff.finite_difference_jacobian(q -> constraint(p1, Quaternion(q..., false), h1, r1, p2, o2, h2, r2), vector(o1))
+Jpq = FiniteDiff.finite_difference_jacobian(q -> constraint(p1, Quaternion(q...), h1, r1, p2, o2, h2, r2), vector(o1))
 
 Jcx = FiniteDiff.finite_difference_jacobian(x -> constraint(p1, o1, h1, r1, x, o2, h2, r2), p2)
-Jcq = FiniteDiff.finite_difference_jacobian(q -> constraint(p1, o1, h1, r1, p2, Quaternion(q..., false), h2, r2), vector(o2))
+Jcq = FiniteDiff.finite_difference_jacobian(q -> constraint(p1, o1, h1, r1, p2, Quaternion(q...), h2, r2), vector(o2))
 
 norm(Jp - [Jpx Jpq])
 norm(Jc - [Jcx Jcq])
@@ -67,11 +67,11 @@ function force_mapping_jacobian(relative, jacobian, xa, qa, ha, ra, xb, qb, hb,
     attjac=false)
     if jacobian == :parent 
         dx = FiniteDiff.finite_difference_jacobian(x -> force_mapping(relative, x, qa, ha, ra, xb, qb, hb, rb) * λ, xa)
-        dq = FiniteDiff.finite_difference_jacobian(q -> force_mapping(relative, xa, Quaternion(q..., false), ha, ra, xb, qb, hb, rb) * λ, vector(qa))
+        dq = FiniteDiff.finite_difference_jacobian(q -> force_mapping(relative, xa, Quaternion(q...), ha, ra, xb, qb, hb, rb) * λ, vector(qa))
         attjac && (dq *= LVᵀmat(qa))
     elseif jacobian == :child 
         dx = FiniteDiff.finite_difference_jacobian(x -> force_mapping(relative, xa, qa, ha, ra, x, qb, hb, rb) * λ, xb)
-        dq = FiniteDiff.finite_difference_jacobian(q -> force_mapping(relative, xa, qa, ha, ra, xb, Quaternion(q..., false), hb, rb) * λ, vector(qb))
+        dq = FiniteDiff.finite_difference_jacobian(q -> force_mapping(relative, xa, qa, ha, ra, xb, Quaternion(q...), hb, rb) * λ, vector(qb))
         attjac && (dq *= LVᵀmat(qa))
     end
     return [dx dq]
@@ -104,11 +104,11 @@ function torque_mapping_jacobian(relative, jacobian, xa, qa, ha, ra, xb, qb, hb,
     attjac=false)
     if jacobian == :parent 
         dx = FiniteDiff.finite_difference_jacobian(x -> torque_mapping(relative, x, qa, ha, ra, xb, qb, hb, rb) * λ, xa)
-        dq = FiniteDiff.finite_difference_jacobian(q -> torque_mapping(relative, xa, Quaternion(q..., false), ha, ra, xb, qb, hb, rb) * λ, vector(qa))
+        dq = FiniteDiff.finite_difference_jacobian(q -> torque_mapping(relative, xa, Quaternion(q...), ha, ra, xb, qb, hb, rb) * λ, vector(qa))
         attjac && (dq *= LVᵀmat(qa))
     elseif jacobian == :child 
         dx = FiniteDiff.finite_difference_jacobian(x -> torque_mapping(relative, xa, qa, ha, ra, x, qb, hb, rb) * λ, xb)
-        dq = FiniteDiff.finite_difference_jacobian(q -> torque_mapping(relative, xa, qa, ha, ra, xb, Quaternion(q..., false), hb, rb) * λ, vector(qb))
+        dq = FiniteDiff.finite_difference_jacobian(q -> torque_mapping(relative, xa, qa, ha, ra, xb, Quaternion(q...), hb, rb) * λ, vector(qb))
         attjac && (dq *= LVᵀmat(qa))
     end
     return [dx dq]

examples/simulation/dev/sphere_sphere_v3.jl

@@ -31,11 +31,11 @@ collision = SphereSphereCollision{Float64,3,3,9}(
         ra, 
         rb)
 
-pa = contact_point(:parent, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false))
-pb = contact_point(:child,  collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false))
+pa = contact_point(:parent, collision, xa, Quaternion(qa...), xb, Quaternion(qb...))
+pb = contact_point(:child,  collision, xa, Quaternion(qa...), xb, Quaternion(qb...))
 
-n = contact_normal(collision,  xa, Quaternion(qa..., false), xb, Quaternion(qb..., false))
-t = contact_tangent(collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false))
+n = contact_normal(collision,  xa, Quaternion(qa...), xb, Quaternion(qb...))
+t = contact_tangent(collision, xa, Quaternion(qa...), xb, Quaternion(qb...))
 
 # sphere a
 sa = GeometryBasics.Sphere(Point(0.0, 0.0, 0.0), ra)
@@ -71,35 +71,35 @@ set_arrow!(vis, pb, -1.0 * vec(t[2, :]), name=:child_tangent2, color=RGBA(0.0, 0
 
 
 
-## Jacobians contact_normal(collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false))
+## Jacobians contact_normal(collision, xa, Quaternion(qa...), xb, Quaternion(qb...))
 
 
-# FiniteDiff.finite_difference_jacobian(x -> vec(contact_normal(collision, x, Quaternion(qa..., false), xb, Quaternion(qb..., false))), xa)
+# FiniteDiff.finite_difference_jacobian(x -> vec(contact_normal(collision, x, Quaternion(qa...), xb, Quaternion(qb...))), xa)
 
 λ = rand(3)
-λ' * FiniteDiff.finite_difference_jacobian(x -> contact_normal(collision, x, Quaternion(qa..., false), xb, Quaternion(qb..., false)), xa)
-λ' * FiniteDiff.finite_difference_jacobian(q -> contact_normal(collision, xa, Quaternion(q..., false), xb, Quaternion(qb..., false)), vector(qa))
+λ' * FiniteDiff.finite_difference_jacobian(x -> contact_normal(collision, x, Quaternion(qa...), xb, Quaternion(qb...)), xa)
+λ' * FiniteDiff.finite_difference_jacobian(q -> contact_normal(collision, xa, Quaternion(q...), xb, Quaternion(qb...)), vector(qa))
 
-∂contact_normal_jvp∂x(:parent, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
-∂contact_normal_jvp∂q(:parent, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
+∂contact_normal_jvp∂x(:parent, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
+∂contact_normal_jvp∂q(:parent, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
 
-contact_tangent(collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false))
+contact_tangent(collision, xa, Quaternion(qa...), xb, Quaternion(qb...))
 
-λ' * FiniteDiff.finite_difference_jacobian(x -> contact_tangent(collision, x, Quaternion(qa..., false), xb, Quaternion(qb..., false))[1, :]', xa)
-λ' * FiniteDiff.finite_difference_jacobian(x -> contact_tangent(collision, x, Quaternion(qa..., false), xb, Quaternion(qb..., false))[2, :]', xa)
+λ' * FiniteDiff.finite_difference_jacobian(x -> contact_tangent(collision, x, Quaternion(qa...), xb, Quaternion(qb...))[1, :]', xa)
+λ' * FiniteDiff.finite_difference_jacobian(x -> contact_tangent(collision, x, Quaternion(qa...), xb, Quaternion(qb...))[2, :]', xa)
 
-λ' * FiniteDiff.finite_difference_jacobian(q -> contact_tangent(collision, xa, Quaternion(q..., false), xb, Quaternion(qb..., false))[1, :], vector(qa))
-λ' * FiniteDiff.finite_difference_jacobian(q -> contact_tangent(collision, xa, Quaternion(q..., false), xb, Quaternion(qb..., false))[2, :], vector(qa))
+λ' * FiniteDiff.finite_difference_jacobian(q -> contact_tangent(collision, xa, Quaternion(q...), xb, Quaternion(qb...))[1, :], vector(qa))
+λ' * FiniteDiff.finite_difference_jacobian(q -> contact_tangent(collision, xa, Quaternion(q...), xb, Quaternion(qb...))[2, :], vector(qa))
 
-∂contact_tangent_jvp∂x(:parent, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
-∂contact_tangent_jvp∂q(:parent, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
+∂contact_tangent_jvp∂x(:parent, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
+∂contact_tangent_jvp∂q(:parent, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
 
-∂contact_tangent_jvp∂x(:child, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
-∂contact_tangent_jvp∂q(:child, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
+∂contact_tangent_jvp∂x(:child, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
+∂contact_tangent_jvp∂q(:child, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
 
 λ = rand(2)
-∂contact_tangent_vjp∂x(:parent, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
-∂contact_tangent_vjp∂q(:parent, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
+∂contact_tangent_vjp∂x(:parent, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
+∂contact_tangent_vjp∂q(:parent, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
 
-∂contact_tangent_vjp∂x(:child, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
-∂contact_tangent_vjp∂q(:child, collision, xa, Quaternion(qa..., false), xb, Quaternion(qb..., false), λ)
+∂contact_tangent_vjp∂x(:child, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)
+∂contact_tangent_vjp∂q(:child, collision, xa, Quaternion(qa...), xb, Quaternion(qb...), λ)

examples/trajectory_optimization/pendulum.jl

@@ -588,11 +588,11 @@ J0 - J1
 
 
 p = srand(3)
-q = Quaternion(normalize(srand(4))..., true)
+q = Quaternion(normalize(srand(4))...)
 inv(q)
 # q = Quaternion(1,0,0,0.0)
-# J0 = FiniteDiff.finite_difference_jacobian(q -> rotation_matrix(inv(Quaternion(q..., true))) * p, vector(q)) * LVᵀmat(q)
-J0 = FiniteDiff.finite_difference_jacobian(q -> rotation_matrix(inv(Quaternion(q..., true))) * p, vector(q))# * LVᵀmat(q)
+# J0 = FiniteDiff.finite_difference_jacobian(q -> rotation_matrix(inv(Quaternion(q...))) * p, vector(q)) * LVᵀmat(q)
+J0 = FiniteDiff.finite_difference_jacobian(q -> rotation_matrix(inv(Quaternion(q...))) * p, vector(q))# * LVᵀmat(q)
 J1 = ∂rotation_matrix_inv∂q(q, p)
 J0 - J1