Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Remove deprecated Quaternion norm and construction #66

Merged
merged 2 commits into from
Nov 23, 2022
Merged

Remove deprecated Quaternion norm and construction #66

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3039a56
Remove deprecated Quaternion norm and construction
janbruedigam Nov 23, 2022
093dc77
Limit Quaternions version to 0.5.7
janbruedigam Nov 23, 2022
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
22 changes: 11 additions & 11 deletions DojoEnvironments/src/panda/methods/dev.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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))
Expand Down Expand Up @@ -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])
Expand Down
2 changes: 1 addition & 1 deletion DojoEnvironments/src/rexhopper/methods/demo.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
2 changes: 1 addition & 1 deletion Project.toml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
4 changes: 2 additions & 2 deletions examples/simulation/dev/capsule_capsule_v3.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions examples/simulation/dev/capsule_capsule_v4.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/simulation/dev/collision_sphere_box.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
2 changes: 1 addition & 1 deletion examples/simulation/dev/collision_sphere_capsule.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
20 changes: 10 additions & 10 deletions examples/simulation/dev/constraint.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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])
Expand All @@ -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]
Expand Down Expand Up @@ -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]
Expand Down
46 changes: 23 additions & 23 deletions examples/simulation/dev/sphere_sphere_v3.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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...), λ)
6 changes: 3 additions & 3 deletions examples/trajectory_optimization/pendulum.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
Loading