-
Notifications
You must be signed in to change notification settings - Fork 25
/
nonlinear.jl
100 lines (79 loc) · 3.57 KB
/
nonlinear.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
"""
NonlinearContact{T,N} <: Contact{T,N}
contact object for impact and friction with a nonlinear friction cone
friction_coefficient: value of friction coefficient
contact_tangent: mapping from world frame to surface tangent frame
contact_normal: inverse/complement of contact_tangent
contact_origin: position of contact on Body relative to center of mass
contact radius: radius of contact
"""
mutable struct NonlinearContact{T,N} <: Contact{T,N}
friction_coefficient::T
friction_parameterization::SMatrix{2,2,T,4}
collision::Collision{T,2,3,6}
end
# function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, contact::NonlinearContact)
# summary(io, contact)
# println(io, "")
# println(io, "friction_coefficient: "*string(contact.friction_coefficient))
# println(io, "friction_parameterization: "*string(contact.friction_parameterization))
# println(io, "collision: "*string(contact.collision))
# end
function NonlinearContact(body::Body{T}, normal::AbstractVector, friction_coefficient;
contact_origin=szeros(T, 3),
contact_radius=0.0) where T
# contact directions
V1, V2, V3 = orthogonal_columns(normal)
A = [V1 V2 V3]
Ainv = inv(A)
contact_normal = Ainv[3, SA[1; 2; 3]]'
contact_tangent = Ainv[SA[1; 2], SA[1; 2; 3]]
# friction parameterization
parameterization = SA{T}[
1.0 0.0
0.0 1.0
]
# collision
collision = SphereHalfSpaceCollision(contact_tangent, contact_normal, SVector{3}(contact_origin), contact_radius)
NonlinearContact{T,8}(friction_coefficient, parameterization, collision)
end
function constraint(mechanism, contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
# contact model
model = contact.model
# parent
pbody = get_body(mechanism, contact.parent_id)
xp, vp, qp, ωp = next_configuration_velocity(pbody.state, mechanism.timestep)
# child
cbody = get_body(mechanism, contact.child_id)
xc, vc, qc, ωc = next_configuration_velocity(cbody.state, mechanism.timestep)
# distance
d = distance(model.collision, xp, qp, xc, qc)
# relative tangential velocity
vt = relative_tangential_velocity(model, xp, qp, vp, ωp, xc, qc, vc, ωc)
# unpack contact variables
γ = contact.impulses[2]
s = contact.impulses_dual[2]
SVector{N½,T}(
d - s[1],
model.friction_coefficient * γ[1] - γ[2],
(model.friction_parameterization * vt - s[SA[3;4]])...)
end
function constraint_jacobian(contact::ContactConstraint{T,N,Nc,Cs,N½}) where {T,N,Nc,Cs<:NonlinearContact{T,N},N½}
friction_coefficient = contact.model.friction_coefficient
γ = contact.impulses[2] + REG * neutral_vector(contact.model) # TODO need to check this is legit
s = contact.impulses_dual[2] + REG * neutral_vector(contact.model) # TODO need to check this is legit
∇s1 = [γ[SA[1]]; szeros(T,3)]'
∇s2 = [szeros(T,3,1) cone_product_jacobian(γ[SA[2,3,4]])]
∇s3 = Diagonal(SVector{4,T}(-1.0, 0.0, -1.0, -1.0))
∇s = [∇s1; ∇s2; ∇s3]
∇γ1 = [s[SA[1]]; szeros(T, 3)]'
∇γ2 = [szeros(T,3,1) cone_product_jacobian(s[SA[2,3,4]])]
∇γ3 = SA[0.0 0.0 0.0 0.0;
friction_coefficient -1.0 0.0 0.0;
0.0 0.0 0.0 0.0;
0.0 0.0 0.0 0.0;]
∇γ = [∇γ1; ∇γ2; ∇γ3]
return [∇s ∇γ]
end
neutral_vector(::NonlinearContact{T,N}) where {T,N} = [sones(T, 2); szeros(T, Int(N/2) - 2)]
cone_degree(::NonlinearContact) = 2