From 6394e9d674426a052c3d02704e9936e89fc79132 Mon Sep 17 00:00:00 2001 From: Tristan Shah <123.tristan.shah@gmail.com> Date: Mon, 13 Nov 2023 10:32:32 -0800 Subject: [PATCH] patched --- scripts/test_grad.jl | 71 +++++++++++++++++++++++++++++++++++++++++ src/Dojo.jl | 4 +++ src/gradients/data.jl | 74 ++++++++++++++++++++++++++++++++++--------- 3 files changed, 134 insertions(+), 15 deletions(-) create mode 100644 scripts/test_grad.jl diff --git a/scripts/test_grad.jl b/scripts/test_grad.jl new file mode 100644 index 000000000..6664c9491 --- /dev/null +++ b/scripts/test_grad.jl @@ -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) \ No newline at end of file diff --git a/src/Dojo.jl b/src/Dojo.jl index 4561d6995..1caf961b6 100644 --- a/src/Dojo.jl +++ b/src/Dojo.jl @@ -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 diff --git a/src/gradients/data.jl b/src/gradients/data.jl index 03e83b0d5..151b00347 100644 --- a/src/gradients/data.jl +++ b/src/gradients/data.jl @@ -149,41 +149,85 @@ function body_constraint_jacobian_joint_data(mechanism::Mechanism{T}, body::Body 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) 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) γ = 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 * γ) + + 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' ∇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) 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) + + cn = contact_normal(model.collision, xp3, qp3, xc3, qc3) + ct = contact_tangent(model.collision, xp3, qp3, xc3, qc3) 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)] ∇compμ = szeros(T,N½,Nd) ∇g = -[∇friction_coefficient ∇contact_radius ∇p]