-
Notifications
You must be signed in to change notification settings - Fork 25
/
utilities.jl
83 lines (69 loc) · 2.75 KB
/
utilities.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
"""
get_sdf(contact, x, q)
returns the signed distance for a contact
contact: ContactConstraint
x: body position
q: body orientation
"""
function get_sdf(contact::ContactConstraint{T,N,Nc,Cs}, x::AbstractVector{T},
q::Quaternion{T}) where {T,N,Nc,Cs<:Contact{T,N}}
model = contact.model
return model.collision.contact_normal * (x + vector_rotate(model.collision.contact_origin, q)) - model.collision.contact_radius
end
function get_sdf(mechanism::Mechanism{T,Nn,Ne,Nb,Ni}, storage::Storage{T,N}) where {T,Nn,Ne,Nb,Ni,N}
d = []
for contact in mechanism.contacts
ibody = get_body(mechanism, contact.parent_id).id - Ne
push!(d, [get_sdf(contact, storage.x[ibody][i], storage.q[ibody][i]) for i = 1:N])
end
return d
end
"""
contact_location(contact, x, q)
location of contact point in world coordinates
contact: ContactConstraint
x: body position
q: body orientation
"""
function contact_location(contact::ContactConstraint{T,N,Nc,Cs}, x::AbstractVector{T},
q::Quaternion{T}) where {T,N,Nc,Cs<:Contact{T,N}}
model = contact.model
return x + vector_rotate(model.collision.contact_origin, q) - model.collision.contact_normal' * model.collision.contact_radius
end
function contact_location(contact::ContactConstraint{T,N,Nc,Cs},
body::Body) where {T,N,Nc,Cs<:Contact{T,N}}
x = body.state.x2
q = body.state.q2
return contact_location(contact, x, q)
end
function contact_location(mechanism::Mechanism, contact::ContactConstraint)
body = mechanism.bodies[findfirst(x -> x.id == contact.parent_id, mechanism.bodies)]
return contact_location(contact, body)
end
function contact_location(mechanism::Mechanism)
return [contact_location(mechanism, contact) for contact in mechanism.contacts]
end
#="""
string_location(contact, x, q)
location of the attach point of the string in world coordinates
collision: StringCollision
x: body position
q: body orientation
"""=#
function string_location(collision::StringCollision, x::AbstractVector{T},
q::Quaternion{T}; relative::Symbol=:parent) where T
origin = (relative == :parent) ? collision.origin_parent : collision.origin_child
return x + vector_rotate(origin, q)
end
function string_location(collision::StringCollision, body::Body; relative::Symbol=:parent)
x = body.state.x2
q = body.state.q2
return string_location(collision, x, q, relative=relative)
end
function string_location(mechanism::Mechanism, collision::StringCollision)
pbody = get_body(mechanism, contact.parent_id)
cbody = get_body(mechanism, contact.child_id)
x_parent = string_location(collision, pbody, relative=:parent)
x_child = string_location(collision, cbody, relative=:child)
return x_parent, x_child
end