Skip to content

Commit 3f79df5

Browse files
janbruedigamjanbruedigam
authored
Merge pull request #95 from gladisor/main
Allows for collisions between spheres and boxes
2 parents 58325cb + 6394e9d commit 3f79df5

File tree

3 files changed

+134
-15
lines changed

3 files changed

+134
-15
lines changed

‎scripts/test_grad.jl

+71
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
using Dojo, StaticArrays
2+
3+
m = 10.0
4+
5+
pad_r = 0.5
6+
pad_m = m * 4/3 * pi * pad_r ^ 3
7+
8+
box_x = 20.0
9+
box_y = 1.0
10+
box_z = 1.0
11+
box_m = m * box_x * box_y * box_z
12+
μ = 0.50
13+
14+
spring = 2000.0
15+
damper = 0.0
16+
17+
origin = Origin()
18+
pad = Dojo.Sphere(pad_r, pad_m)
19+
box = Dojo.Box(box_x, box_y, box_z, box_m)
20+
bodies = [pad, box]
21+
22+
# pad_joint = JointConstraint(Planar(
23+
# origin, pad, Y_AXIS,
24+
# spring = spring,
25+
# damper = damper,
26+
# child_vertex = - Z_AXIS * (pad_r + box_z),
27+
# ))
28+
29+
pad_joint = JointConstraint(Prismatic(
30+
origin, pad, Dojo.X_AXIS,
31+
spring = spring,
32+
damper = damper,
33+
child_vertex = - Dojo.Z_AXIS * (pad_r + box_z),
34+
))
35+
36+
box_joint = JointConstraint(Prismatic(
37+
origin, box, Dojo.X_AXIS,
38+
child_vertex = -Dojo.Z_AXIS * box_z / 2 .+ Dojo.X_AXIS * box_x / 2 .- 2.0 * Dojo.X_AXIS * pad_r))
39+
40+
joints = [pad_joint, box_joint]
41+
42+
collision = SphereBoxCollision{Float64, 2, 3, 6}(szeros(3), box_x, box_y, box_z, pad_r)
43+
friction_parameterization = SA{Float64}[
44+
1.0 0.0
45+
0.0 1.0]
46+
contact = NonlinearContact{Float64, 8}(μ, friction_parameterization, collision)
47+
contacts = [ContactConstraint((contact, pad.id, box.id), name = :pad_belt)]
48+
49+
mech = Mechanism(origin, bodies, joints, contacts)
50+
51+
zero_coordinates!(mech)
52+
zero_velocities!(mech)
53+
# set_maximal_configurations!(pad, x = Z_AXIS * pad_r * 5.0)
54+
55+
u = zeros(input_dimension(mech))
56+
u[1] = 0.0 #-100#-100#0.0 ## z axis force on pad
57+
# u[2] = 0.0 ## x axis force on pad
58+
# u[3] = 26.0 ## x axis force on box
59+
u[2] = 380.0
60+
61+
K = 1000
62+
storage = Storage(K, length(mech.bodies))
63+
64+
for k in 1:K
65+
Jx, Ju = get_minimal_gradients!(mech, get_minimal_state(mech), u)
66+
Dojo.save_to_storage!(mech, storage, k)
67+
end
68+
69+
vis = Visualizer()
70+
open(vis)
71+
visualize(mech, storage, vis = vis)

‎src/Dojo.jl

+4
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,10 @@ module Dojo
33
# constants
44
global const REG = 1.0e-10::Float64
55

6+
global const X_AXIS = [1;0;0]
7+
global const Y_AXIS = [0;1;0]
8+
global const Z_AXIS = [0;0;1]
9+
610
#TODO: remove
711
using FiniteDiff
812

‎src/gradients/data.jl

+59-15
Original file line numberDiff line numberDiff line change
@@ -149,41 +149,85 @@ function body_constraint_jacobian_joint_data(mechanism::Mechanism{T}, body::Body
149149
return [∇u ∇spring ∇damper]
150150
end
151151

152-
function body_constraint_jacobian_contact_data(mechanism::Mechanism, body::Body{T},
153-
contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
154-
Nd = data_dim(contact)
152+
# function body_constraint_jacobian_contact_data(mechanism::Mechanism, body::Body{T},
153+
# contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
154+
# Nd = data_dim(contact)
155+
# model = contact.model
156+
# contact_radius = model.collision.contact_radius
157+
# offset = model.collision.contact_normal' * contact_radius
158+
# xp3, qp3 = next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
159+
# xc3, qc3 = next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)
160+
161+
# γ = contact.impulses[2]
162+
163+
# ∇friction_coefficient = szeros(T,3,1)
164+
165+
# X = force_mapping(:parent, model, xp3, qp3, xc3, qc3)
166+
# ∇p = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ)
167+
# ∇contact_radius = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ) * -rotation_matrix(inv(qp3)) * model.collision.contact_normal'
168+
169+
# ∇X = szeros(T,3,Nd)
170+
# ∇Q = -[∇friction_coefficient ∇contact_radius ∇p]
171+
# return [∇X; ∇Q]
172+
# end
173+
174+
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½}
175+
Nd = Dojo.data_dim(contact)
155176
model = contact.model
156-
contact_radius = model.collision.contact_radius
157-
offset = model.collision.contact_normal' * contact_radius
158-
xp3, qp3 = next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
159-
xc3, qc3 = next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)
177+
xp3, qp3 = Dojo.next_configuration(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
178+
xc3, qc3 = Dojo.next_configuration(get_body(mechanism, contact.child_id).state, mechanism.timestep)
160179

161180
γ = contact.impulses[2]
162181

163182
∇friction_coefficient = szeros(T,3,1)
164183

165-
X = force_mapping(:parent, model, xp3, qp3, xc3, qc3)
166-
∇p = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ)
167-
∇contact_radius = - ∂skew∂p(VRmat(qp3) * LᵀVᵀmat(qp3) * X * γ) * -rotation_matrix(inv(qp3)) * model.collision.contact_normal'
184+
X = Dojo.force_mapping(:parent, model, xp3, qp3, xc3, qc3)
185+
∇p = - Dojo.∂skew∂p(Dojo.VRmat(qp3) * Dojo.LᵀVᵀmat(qp3) * X * γ)
186+
187+
cn = contact_normal(model.collision, xp3, qp3, xc3, qc3)
188+
∇contact_radius = - Dojo.∂skew∂p(Dojo.VRmat(qp3) * Dojo.LᵀVᵀmat(qp3) * X * γ) * -Dojo.rotation_matrix(inv(qp3)) * cn'
168189

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

195+
# 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½}
196+
# Nd = data_dim(contact)
197+
# model = contact.model
198+
199+
# xp3, vp25, qp3, ωp25 = next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
200+
# xc3, vc25, qc3, ωc25 = next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)
201+
202+
# s = contact.impulses_dual[2]
203+
# γ = contact.impulses[2]
204+
205+
# ∇friction_coefficient = SA[0,γ[1],0,0]
206+
# ∇contact_radius = [-model.collision.contact_normal; szeros(T,1,3); -model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3))] * model.collision.contact_normal'
207+
# ∇p = [model.collision.contact_normal * rotation_matrix(qp3); szeros(T,1,3); model.collision.contact_tangent * skew(vector_rotate(ωp25, qp3)) * rotation_matrix(qp3)]
208+
209+
# ∇compμ = szeros(T,N½,Nd)
210+
# ∇g = -[∇friction_coefficient ∇contact_radius ∇p]
211+
212+
# return [∇compμ; ∇g]
213+
# end
214+
174215
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½}
175-
Nd = data_dim(contact)
216+
Nd = Dojo.data_dim(contact)
176217
model = contact.model
177218

178-
xp3, vp25, qp3, ωp25 = next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
179-
xc3, vc25, qc3, ωc25 = next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)
219+
xp3, vp25, qp3, ωp25 = Dojo.next_configuration_velocity(get_body(mechanism, contact.parent_id).state, mechanism.timestep)
220+
xc3, vc25, qc3, ωc25 = Dojo.next_configuration_velocity(get_body(mechanism, contact.child_id).state, mechanism.timestep)
221+
222+
cn = contact_normal(model.collision, xp3, qp3, xc3, qc3)
223+
ct = contact_tangent(model.collision, xp3, qp3, xc3, qc3)
180224

181225
s = contact.impulses_dual[2]
182226
γ = contact.impulses[2]
183227

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

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

0 commit comments

Comments
 (0)