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

Allows for collisions between spheres and boxes #95

Merged
merged 1 commit into from
Mar 20, 2024
Merged

Allows for collisions between spheres and boxes #95

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
71 changes: 71 additions & 0 deletions scripts/test_grad.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,71 @@
using Dojo, StaticArrays

m = 10.0

pad_r = 0.5
pad_m = m * 4/3 * pi * pad_r ^ 3

box_x = 20.0
box_y = 1.0
box_z = 1.0
box_m = m * box_x * box_y * box_z
μ = 0.50

spring = 2000.0
damper = 0.0

origin = Origin()
pad = Dojo.Sphere(pad_r, pad_m)
box = Dojo.Box(box_x, box_y, box_z, box_m)
bodies = [pad, box]

# pad_joint = JointConstraint(Planar(
# origin, pad, Y_AXIS,
# spring = spring,
# damper = damper,
# child_vertex = - Z_AXIS * (pad_r + box_z),
# ))

pad_joint = JointConstraint(Prismatic(
origin, pad, Dojo.X_AXIS,
spring = spring,
damper = damper,
child_vertex = - Dojo.Z_AXIS * (pad_r + box_z),
))

box_joint = JointConstraint(Prismatic(
origin, box, Dojo.X_AXIS,
child_vertex = -Dojo.Z_AXIS * box_z / 2 .+ Dojo.X_AXIS * box_x / 2 .- 2.0 * Dojo.X_AXIS * pad_r))

joints = [pad_joint, box_joint]

collision = SphereBoxCollision{Float64, 2, 3, 6}(szeros(3), box_x, box_y, box_z, pad_r)
friction_parameterization = SA{Float64}[
1.0 0.0
0.0 1.0]
contact = NonlinearContact{Float64, 8}(μ, friction_parameterization, collision)
contacts = [ContactConstraint((contact, pad.id, box.id), name = :pad_belt)]

mech = Mechanism(origin, bodies, joints, contacts)

zero_coordinates!(mech)
zero_velocities!(mech)
# set_maximal_configurations!(pad, x = Z_AXIS * pad_r * 5.0)

u = zeros(input_dimension(mech))
u[1] = 0.0 #-100#-100#0.0 ## z axis force on pad
# u[2] = 0.0 ## x axis force on pad
# u[3] = 26.0 ## x axis force on box
u[2] = 380.0

K = 1000
storage = Storage(K, length(mech.bodies))

for k in 1:K
Jx, Ju = get_minimal_gradients!(mech, get_minimal_state(mech), u)
Dojo.save_to_storage!(mech, storage, k)
end

vis = Visualizer()
open(vis)
visualize(mech, storage, vis = vis)
4 changes: 4 additions & 0 deletions src/Dojo.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,10 @@ module Dojo
# constants
global const REG = 1.0e-10::Float64

global const X_AXIS = [1;0;0]
global const Y_AXIS = [0;1;0]
global const Z_AXIS = [0;0;1]

#TODO: remove
using FiniteDiff

Expand Down
74 changes: 59 additions & 15 deletions src/gradients/data.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,41 +149,85 @@
return [∇u ∇spring ∇damper]
end

function body_constraint_jacobian_contact_data(mechanism::Mechanism, body::Body{T},
contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
Nd = data_dim(contact)
# function body_constraint_jacobian_contact_data(mechanism::Mechanism, body::Body{T},
# contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
# Nd = data_dim(contact)
# model = contact.model
# contact_radius = model.collision.contact_radius
# offset = model.collision.contact_normal' * contact_radius
# xp3, qp3 = next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
# xc3, qc3 = next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)

# γ = contact.impulses[2]

# ∇friction_coefficient = szeros(T,3,1)

# X = force_mapping(:parent, model, xp3, qp3, xc3, qc3)
# ∇p = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ)
# ∇contact_radius = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ) * -rotation_matrix(inv(qp3)) * model.collision.contact_normal'

# ∇X = szeros(T,3,Nd)
# ∇Q = -[∇friction_coefficient ∇contact_radius ∇p]
# return [∇X; ∇Q]
# end

function body_constraint_jacobian_contact_data(mechanism::Mechanism, body::Body{T}, contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
Nd = Dojo.data_dim(contact)

Check warning on line 175 in src/gradients/data.jl

View check run for this annotation

Codecov / codecov/patch

src/gradients/data.jl#L174-L175 

Added lines #L174 - L175 were not covered by tests
model = contact.model
contact_radius = model.collision.contact_radius
offset = model.collision.contact_normal' * contact_radius
xp3, qp3 = next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
xc3, qc3 = next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)
xp3, qp3 = Dojo.next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
xc3, qc3 = Dojo.next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)

Check warning on line 178 in src/gradients/data.jl

View check run for this annotation

Codecov / codecov/patch

src/gradients/data.jl#L177-L178 

Added lines #L177 - L178 were not covered by tests

γ = contact.impulses[2]

∇friction_coefficient = szeros(T,3,1)

X = force_mapping(:parent, model, xp3, qp3, xc3, qc3)
∇p = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ)
∇contact_radius = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ) * -rotation_matrix(inv(qp3)) * model.collision.contact_normal'
X = Dojo.force_mapping(:parent, model, xp3, qp3, xc3, qc3)
∇p = - Dojo.∂skew∂p(Dojo.VRmat(qp3) * Dojo.LᵀVᵀmat(qp3) * X * γ)

Check warning on line 185 in src/gradients/data.jl

View check run for this annotation

Codecov / codecov/patch

src/gradients/data.jl#L184-L185 

Added lines #L184 - L185 were not covered by tests

cn = contact_normal(model.collision, xp3, qp3, xc3, qc3)
∇contact_radius = - Dojo.∂skew∂p(Dojo.VRmat(qp3) * Dojo.LᵀVᵀmat(qp3) * X * γ) * -Dojo.rotation_matrix(inv(qp3)) * cn'

Check warning on line 188 in src/gradients/data.jl

View check run for this annotation

Codecov / codecov/patch

src/gradients/data.jl#L187-L188 

Added lines #L187 - L188 were not covered by tests

∇X = szeros(T,3,Nd)
∇Q = -[∇friction_coefficient ∇contact_radius ∇p]
return [∇X; ∇Q]
end

# function contact_constraint_jacobian_contact_data(mechanism::Mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}, body::Body{T}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
# Nd = data_dim(contact)
# model = contact.model

# xp3, vp25, qp3, ωp25 = next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
# xc3, vc25, qc3, ωc25 = next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)

# s = contact.impulses_dual[2]
# γ = contact.impulses[2]

# ∇friction_coefficient = SA[0,γ[1],0,0]
# ∇contact_radius = [-model.collision.contact_normal; szeros(T,1,3); -model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3))] * model.collision.contact_normal'
# ∇p = [model.collision.contact_normal * rotation_matrix(qp3); szeros(T,1,3); model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3)) * rotation_matrix(qp3)]

# ∇compμ = szeros(T,N½,Nd)
# ∇g = -[∇friction_coefficient ∇contact_radius ∇p]

# return [∇compμ; ∇g]
# end

function contact_constraint_jacobian_contact_data(mechanism::Mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}, body::Body{T}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
Nd = data_dim(contact)
Nd = Dojo.data_dim(contact)

Check warning on line 216 in src/gradients/data.jl

View check run for this annotation

Codecov / codecov/patch

src/gradients/data.jl#L216 

Added line #L216 was not covered by tests
model = contact.model

xp3, vp25, qp3, ωp25 = next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
xc3, vc25, qc3, ωc25 = next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)
xp3, vp25, qp3, ωp25 = Dojo.next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
xc3, vc25, qc3, ωc25 = Dojo.next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)

Check warning on line 220 in src/gradients/data.jl

View check run for this annotation

Codecov / codecov/patch

src/gradients/data.jl#L219-L220 

Added lines #L219 - L220 were not covered by tests

cn = contact_normal(model.collision, xp3, qp3, xc3, qc3)
ct = contact_tangent(model.collision, xp3, qp3, xc3, qc3)

Check warning on line 223 in src/gradients/data.jl

View check run for this annotation

Codecov / codecov/patch

src/gradients/data.jl#L222-L223 

Added lines #L222 - L223 were not covered by tests

s = contact.impulses_dual[2]
γ = contact.impulses[2]

∇friction_coefficient = SA[0,γ[1],0,0]
∇contact_radius = [-model.collision.contact_normal; szeros(T,1,3); -model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3))] * model.collision.contact_normal'
∇p = [model.collision.contact_normal * rotation_matrix(qp3); szeros(T,1,3); model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3)) * rotation_matrix(qp3)]
∇contact_radius = [-cn; szeros(T,1,3); -ct * Dojo.skew(Dojo.vector_rotate(ωp25, qp3))] * cn'
∇p = [cn * Dojo.rotation_matrix(qp3); szeros(T,1,3); ct * Dojo.skew(Dojo.vector_rotate(ωp25, qp3)) * Dojo.rotation_matrix(qp3)]

Check warning on line 230 in src/gradients/data.jl

View check run for this annotation

Codecov / codecov/patch

src/gradients/data.jl#L229-L230 

Added lines #L229 - L230 were not covered by tests

∇compμ = szeros(T,N½,Nd)
∇g = -[∇friction_coefficient ∇contact_radius ∇p]
Expand Down
Loading