diff --git a/DojoEnvironments/src/environments.jl b/DojoEnvironments/src/environments.jl index e90328397..08abcf746 100644 --- a/DojoEnvironments/src/environments.jl +++ b/DojoEnvironments/src/environments.jl @@ -30,19 +30,36 @@ end state: provided state """ function state_map(::Environment, state) - return x + return state end """ input_map(environment, input) - maps the provided input to the environments internal input + maps the provided input to the environment's internal input environment: environment input: provided input """ -function input_map(::Environment, u) - return u +function input_map(::Environment, input) + return input +end + +function input_map(environment::Environment, ::Nothing) + return zeros(input_dimension(environment.mechanism)) +end + +""" + set_input!(environment, input) + + sets the provided input to the environment's mechanism + + environment: environment + input: provided input +""" +function Dojo.set_input!(environment::Environment, input) + set_input!(environment.mechanism, input_map(environment, input)) + return end """ @@ -57,10 +74,10 @@ end record: record step in storage opts: SolverOptions """ -function Dojo.step!(environment::Environment, x, u=nothing; k=1, record=false, opts=SolverOptions()) - x = state_map(environment, x) - u = input_map(environment, u) - Dojo.step_minimal_coordinates!(environment.mechanism, x, u; opts) +function Dojo.step!(environment::Environment, state, input=nothing; k=1, record=false, opts=SolverOptions()) + state = state_map(environment, state) + input = input_map(environment, input) + Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts) record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k) return @@ -75,8 +92,9 @@ end controller!: Control function kwargs: same as for Dojo.simulate """ -function Dojo.simulate!(environment::Environment{T,N}, controller! = (mechanism, k) -> nothing; kwargs...) where {T,N} - simulate!(environment.mechanism, 1:N, environment.storage, controller!; kwargs...) +function Dojo.simulate!(environment::Environment{T,N}, controller! = (environment, k) -> nothing; kwargs...) where {T,N} + controller_wrapper!(mechanism, k) = controller!(environment, k) + simulate!(environment.mechanism, 1:N, environment.storage, controller_wrapper!; kwargs...) end """ @@ -90,6 +108,18 @@ function get_state(environment::Environment) return get_minimal_state(environment.mechanism) end +""" + initialize!(environment; kwargs...) + + initializes the environment's mechanism + + environment: Environment + kwargs: same as for DojoEnvironments' mechanisms +""" +function Dojo.initialize!(environment::Environment, model; kwargs...) + eval(Symbol(:initialize, :_, string_to_symbol(model), :!))(environment.mechanism; kwargs...) +end + """ visualize(environment; kwargs...) diff --git a/DojoEnvironments/src/environments/ant_ars.jl b/DojoEnvironments/src/environments/ant_ars.jl index de17f4922..59cf104f0 100644 --- a/DojoEnvironments/src/environments/ant_ars.jl +++ b/DojoEnvironments/src/environments/ant_ars.jl @@ -13,7 +13,6 @@ function ant_ars(; dampers=0, parse_springs=true, parse_dampers=true, - limits=true, joint_limits=Dict([ (:hip_1, [-30,30] * π / 180), (:ankle_1, [30,70] * π / 180), @@ -38,7 +37,6 @@ function ant_ars(; dampers, parse_springs, parse_dampers, - limits, joint_limits, keep_fixed_joints, friction_coefficient, diff --git a/DojoEnvironments/src/environments/cartpole_dqn.jl b/DojoEnvironments/src/environments/cartpole_dqn.jl index ee7871891..ad1daeb76 100644 --- a/DojoEnvironments/src/environments/cartpole_dqn.jl +++ b/DojoEnvironments/src/environments/cartpole_dqn.jl @@ -10,14 +10,13 @@ function cartpole_dqn(; gravity=-9.81, slider_mass=1, pendulum_mass=1, - pendulum_length=1, + link_length=1, radius=0.075, color=RGBA(0.7, 0.7, 0.7, 1), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, T=Float64) mechanism = get_cartpole(; @@ -26,12 +25,11 @@ function cartpole_dqn(; gravity, slider_mass, pendulum_mass, - pendulum_length, + link_length, radius, color, springs, dampers, - limits, joint_limits, keep_fixed_joints, T diff --git a/DojoEnvironments/src/environments/include.jl b/DojoEnvironments/src/environments/include.jl index dd550a175..7e4d004f5 100644 --- a/DojoEnvironments/src/environments/include.jl +++ b/DojoEnvironments/src/environments/include.jl @@ -1,4 +1,8 @@ include("ant_ars.jl") include("cartpole_dqn.jl") include("pendulum.jl") -include("quadruped_sampling.jl") \ No newline at end of file +include("quadruped_waypoint.jl") +include("quadruped_sampling.jl") +include("quadrotor_waypoint.jl") +include("uuv_waypoint.jl") +include("youbot_waypoint.jl") \ No newline at end of file diff --git a/DojoEnvironments/src/environments/pendulum.jl b/DojoEnvironments/src/environments/pendulum.jl index e241adf03..ed628fff3 100644 --- a/DojoEnvironments/src/environments/pendulum.jl +++ b/DojoEnvironments/src/environments/pendulum.jl @@ -9,11 +9,10 @@ function pendulum(; input_scaling=timestep, gravity=-9.81, mass=1, - length=1, + link_length=1, color=RGBA(1, 0, 0), springs=0, dampers=0, - limits=false, joint_limits=Dict(), spring_offset=szeros(1), orientation_offset=one(Quaternion), @@ -24,18 +23,17 @@ function pendulum(; input_scaling, gravity, mass, - length, + link_length, color, springs, dampers, - limits, joint_limits, spring_offset, orientation_offset, T ) - storage = Storage(horizon, Base.length(mechanism.bodies)) + storage = Storage(horizon, length(mechanism.bodies)) return Pendulum{T,horizon}(mechanism, storage) end diff --git a/DojoEnvironments/src/environments/quadrotor_waypoint.jl b/DojoEnvironments/src/environments/quadrotor_waypoint.jl new file mode 100644 index 000000000..71a542593 --- /dev/null +++ b/DojoEnvironments/src/environments/quadrotor_waypoint.jl @@ -0,0 +1,164 @@ +mutable struct QuadrotorWaypoint{T,N} <: Environment{T,N} + mechanism::Mechanism{T} + storage::Storage{T,N} + + rpms::AbstractVector +end + +function quadrotor_waypoint(; + horizon=100, + timestep=0.01, + input_scaling=timestep, + gravity=-9.81, + urdf=:pelican, + springs=0, + dampers=0, + parse_springs=true, + parse_dampers=true, + joint_limits=Dict(), + keep_fixed_joints=false, + friction_coefficient=0.5, + contact_rotors=true, + contact_body=true, + T=Float64) + + mechanism = get_quadrotor(; + timestep, + input_scaling, + gravity, + urdf, + springs, + dampers, + parse_springs, + parse_dampers, + joint_limits, + keep_fixed_joints, + friction_coefficient, + contact_rotors, + contact_body, + T + ) + + storage = Storage(horizon, length(mechanism.bodies)) + + return QuadrotorWaypoint{T,horizon}(mechanism, storage, zeros(4)) +end + +function state_map(::QuadrotorWaypoint, state) + state = [state;zeros(8)] + return state +end + +function input_map(environment::QuadrotorWaypoint, input) + # Input is rotor rpm directly + # Rotors are only visualized, dynamics are mapped here + environment.rpms = input + + body = get_body(environment.mechanism, :base_link) + q = body.state.q2 + + force_torque = rpm_to_force_torque(environment, input, q) + + input = [force_torque;zeros(4)] + + return input +end + +function input_map(::QuadrotorWaypoint, ::Nothing) + return zeros(10) +end + +function Dojo.step!(environment::QuadrotorWaypoint, state, input=nothing; k=1, record=false, opts=SolverOptions()) + state = state_map(environment, state) + input = input_map(environment, input) + Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts) + record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k) + + return +end + +function Dojo.simulate!(environment::QuadrotorWaypoint{T,N}, controller! = (environment, k) -> nothing; kwargs...) where {T,N} + mechanism = environment.mechanism + + joint_rotor_0 = get_joint(mechanism, :rotor_0_joint) + joint_rotor_1 = get_joint(mechanism, :rotor_1_joint) + joint_rotor_2 = get_joint(mechanism, :rotor_2_joint) + joint_rotor_3 = get_joint(mechanism, :rotor_3_joint) + + function controller_wrapper!(mechanism, k) + rpms = environment.rpms + set_minimal_velocities!(mechanism, joint_rotor_0, [rpms[1]]) + set_minimal_velocities!(mechanism, joint_rotor_1, [-rpms[2]]) + set_minimal_velocities!(mechanism, joint_rotor_2, [rpms[3]]) + set_minimal_velocities!(mechanism, joint_rotor_3, [-rpms[4]]) + + controller!(environment, k) + end + + simulate!(environment.mechanism, 1:N, environment.storage, controller_wrapper!; kwargs...) +end + +function get_state(environment::QuadrotorWaypoint) + state = get_minimal_state(environment.mechanism)[1:12] + return state +end + +function Dojo.visualize(environment::QuadrotorWaypoint; return_animation=false, kwargs...) + vis, animation = visualize(environment.mechanism, environment.storage; return_animation=true, kwargs...) + + waypoints = [ + [1;1;0.3;pi/4], + [2;0;0.3;-pi/4], + [1;-1;0.3;-3*pi/4], + [0;0;0.3;-5*pi/4], + ] + for i=1:4 + waypoint_shape = Sphere(0.2;color=RGBA(0,0.25*i,0,0.3)) + visshape = Dojo.convert_shape(waypoint_shape) + subvisshape = vis["waypoints"]["waypoint$i"] + Dojo.setobject!(subvisshape, visshape, waypoint_shape) + Dojo.atframe(animation, 1) do + Dojo.set_node!(waypoints[i][1:3], one(Quaternion), waypoint_shape, subvisshape, true) + end + end + Dojo.setanimation!(vis,animation) + + return_animation ? (return vis, animation) : (return vis) +end + +# ## physics functions + +function rpm_to_force_torque(::QuadrotorWaypoint, rpm::Real, rotor_sign::Int64) + force_factor = 0.001 + torque_factor = 0.0001 + + force = sign(rpm)*force_factor*rpm^2 + torque = sign(rpm)*rotor_sign*torque_factor*rpm^2 + + return [force;0;0], [torque;0;0] +end +function rpm_to_force_torque(environment::QuadrotorWaypoint, rpms::AbstractVector, q::Quaternion) + qympi2 = Dojo.RotY(-pi/2) + orientations = [qympi2;qympi2;qympi2;qympi2] + directions = [1;-1;1;-1] + force_vertices = [ + [0.21; 0; 0.05], + [0; 0.21; 0.05], + [-0.21; 0; 0.05], + [0; -0.21; 0.05], + ] + + forces_torques = [rpm_to_force_torque(environment, rpms[i], directions[i]) for i=1:4] + forces = getindex.(forces_torques,1) + torques = getindex.(forces_torques,2) + + forces = Dojo.vector_rotate.(forces, orientations) # in local frame + torques = Dojo.vector_rotate.(torques, orientations) # in local frame + + torques_from_forces = Dojo.cross.(force_vertices, forces) + + force = Dojo.vector_rotate(sum(forces), q) # in minimal frame + torque = Dojo.vector_rotate(sum(torques .+ torques_from_forces), q) # in minimal frame + + return [force; torque] +end diff --git a/DojoEnvironments/src/environments/quadruped_sampling.jl b/DojoEnvironments/src/environments/quadruped_sampling.jl index 46fc66b9c..8ec0244f2 100644 --- a/DojoEnvironments/src/environments/quadruped_sampling.jl +++ b/DojoEnvironments/src/environments/quadruped_sampling.jl @@ -14,13 +14,12 @@ function quadruped_sampling(; parse_springs=true, parse_dampers=true, spring_offset=true, - limits=true, joint_limits=Dict(vcat([[ (Symbol(group,:_hip_joint), [-0.5,0.5]), (Symbol(group,:_thigh_joint), [-0.5,1.5]), (Symbol(group,:_calf_joint), [-2.5,-1])] for group in [:FR, :FL, :RR, :RL]]...)), - keep_fixed_joints=true, + keep_fixed_joints=false, friction_coefficient=0.8, contact_feet=true, contact_body=true, @@ -36,7 +35,6 @@ function quadruped_sampling(; parse_springs, parse_dampers, spring_offset, - limits, joint_limits, keep_fixed_joints, friction_coefficient, diff --git a/DojoEnvironments/src/environments/quadruped_waypoint.jl b/DojoEnvironments/src/environments/quadruped_waypoint.jl new file mode 100644 index 000000000..bad66ff15 --- /dev/null +++ b/DojoEnvironments/src/environments/quadruped_waypoint.jl @@ -0,0 +1,102 @@ +mutable struct QuadrupedWaypoint{T,N} <: Environment{T,N} + mechanism::Mechanism{T} + storage::Storage{T,N} +end + +function quadruped_waypoint(; + horizon=100, + timestep=0.001, + input_scaling=timestep, + gravity=-9.81, + urdf=:gazebo_a1, + springs=0, + dampers=0, + parse_springs=true, + parse_dampers=true, + spring_offset=true, + joint_limits=Dict(vcat([[ + (Symbol(group,:_hip_joint), [-0.5,0.5]), + (Symbol(group,:_thigh_joint), [-0.5,1.5]), + (Symbol(group,:_calf_joint), [-2.5,-1])] + for group in [:FR, :FL, :RR, :RL]]...)), + keep_fixed_joints=false, + friction_coefficient=0.8, + contact_feet=true, + contact_body=false, + T=Float64) + + mechanism = get_quadruped(; + timestep, + input_scaling, + gravity, + urdf, + springs, + dampers, + parse_springs, + parse_dampers, + spring_offset, + joint_limits, + keep_fixed_joints, + friction_coefficient, + contact_feet, + contact_body, + T + ) + + storage = Storage(horizon, length(mechanism.bodies)) + + return QuadrupedWaypoint{T,horizon}(mechanism, storage) +end + +function state_map(::QuadrupedWaypoint, state) + return state +end + +function input_map(::QuadrupedWaypoint, input) + input = [zeros(6);input] # trunk not actuated + return input +end + +function input_map(::QuadrupedWaypoint, ::Nothing) + input = zeros(18) + return input +end + +function Dojo.step!(environment::QuadrupedWaypoint, state, input=nothing; k=1, record=false, opts=SolverOptions()) + state = state_map(environment, state) + input = input_map(environment, input) + Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts) + record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k) + + return +end + +function get_state(environment::QuadrupedWaypoint) + state = get_minimal_state(environment.mechanism) + + # x: floating base, FR (hip, thigh, calf), FL, RR, RL + return state +end + +function Dojo.visualize(environment::QuadrupedWaypoint; return_animation=false, kwargs...) + vis, animation = visualize(environment.mechanism, environment.storage; return_animation=true, kwargs...) + + waypoints = [ + [0.5;0.5;0.3;pi/4], + [1;0;0.3;-pi/4], + [0.5;-0.5;0.3;-3*pi/4], + [0;0;0.3;-5*pi/4], + ] + for i=1:4 + waypoint_shape = Sphere(0.2;color=RGBA(0,0.25*i,0,0.3)) + visshape = Dojo.convert_shape(waypoint_shape) + subvisshape = vis["waypoints"]["waypoint$i"] + Dojo.setobject!(subvisshape, visshape, waypoint_shape) + Dojo.atframe(animation, 1) do + Dojo.set_node!(waypoints[i][1:3], one(Quaternion), waypoint_shape, subvisshape, true) + end + end + Dojo.setanimation!(vis,animation) + + return_animation ? (return vis, animation) : (return vis) +end \ No newline at end of file diff --git a/DojoEnvironments/src/environments/uuv_waypoint.jl b/DojoEnvironments/src/environments/uuv_waypoint.jl new file mode 100644 index 000000000..1daa957ec --- /dev/null +++ b/DojoEnvironments/src/environments/uuv_waypoint.jl @@ -0,0 +1,183 @@ +mutable struct UUVWaypoint{T,N} <: Environment{T,N} + mechanism::Mechanism{T} + storage::Storage{T,N} + + rpms::AbstractVector +end + +function uuv_waypoint(; + horizon=100, + timestep=0.01, + input_scaling=timestep, + gravity=-9.81, + urdf=:mini_tortuga, + springs=0, + dampers=0, + parse_springs=true, + parse_dampers=true, + joint_limits=Dict(), + keep_fixed_joints=false, + friction_coefficient=0.5, + contact_body=true, + T=Float64) + + mechanism = get_uuv(; + timestep, + input_scaling, + gravity, + urdf, + springs, + dampers, + parse_springs, + parse_dampers, + joint_limits, + keep_fixed_joints, + friction_coefficient, + contact_body, + T + ) + + storage = Storage(horizon, length(mechanism.bodies)) + + return UUVWaypoint{T,horizon}(mechanism, storage, zeros(6)) +end + +function state_map(::UUVWaypoint, state) + state = [state;zeros(12)] + return state +end + +function input_map(environment::UUVWaypoint, input) + # Input is rotor rpm directly + # Rotors are only visualized, dynamics are mapped here + environment.rpms = input + + body = get_body(environment.mechanism, :base_link) + q = body.state.q2 + + force_torque = rpm_to_force_torque(environment, input, q) + + input = [force_torque;zeros(6)] + + return input +end + +function input_map(::UUVWaypoint, ::Nothing) + return zeros(12) +end + +function Dojo.step!(environment::UUVWaypoint, state, input=nothing; k=1, record=false, opts=SolverOptions()) + state = state_map(environment, state) + input = input_map(environment, input) + Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts) + record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k) + + return +end + +function Dojo.simulate!(environment::UUVWaypoint{T,N}, controller! = (environment, k) -> nothing; kwargs...) where {T,N} + mechanism = environment.mechanism + + rotor_1_joint = get_joint(mechanism, :rotor_1_joint) + rotor_2_joint = get_joint(mechanism, :rotor_2_joint) + rotor_3_joint = get_joint(mechanism, :rotor_3_joint) + rotor_4_joint = get_joint(mechanism, :rotor_4_joint) + rotor_5_joint = get_joint(mechanism, :rotor_5_joint) + rotor_6_joint = get_joint(mechanism, :rotor_6_joint) + + function controller_wrapper!(mechanism, k) + rpms = environment.rpms + set_minimal_velocities!(mechanism, rotor_1_joint, [rpms[1]]) + set_minimal_velocities!(mechanism, rotor_2_joint, [rpms[2]]) + set_minimal_velocities!(mechanism, rotor_3_joint, [-rpms[3]]) + set_minimal_velocities!(mechanism, rotor_4_joint, [-rpms[4]]) + set_minimal_velocities!(mechanism, rotor_5_joint, [rpms[5]]) + set_minimal_velocities!(mechanism, rotor_6_joint, [-rpms[6]]) + + buoyancy!(environment, mechanism) + + controller!(environment, k) + end + + simulate!(environment.mechanism, 1:N, environment.storage, controller_wrapper!; kwargs...) +end + +function get_state(environment::UUVWaypoint) + state = get_minimal_state(environment.mechanism)[1:12] + return state +end + +function Dojo.visualize(environment::UUVWaypoint; return_animation=false, kwargs...) + vis, animation = visualize(environment.mechanism, environment.storage; return_animation=true, kwargs...) + + waypoints = [ + [1;1;0.3;pi/4], + [2;0;0.3;-pi/4], + [1;-1;0.3;-3*pi/4], + [0;0;0.3;-5*pi/4], + ] + for i=1:4 + waypoint_shape = Sphere(0.2;color=RGBA(0,0.25*i,0,0.3)) + visshape = Dojo.convert_shape(waypoint_shape) + subvisshape = vis["waypoints"]["waypoint$i"] + Dojo.setobject!(subvisshape, visshape, waypoint_shape) + Dojo.atframe(animation, 1) do + Dojo.set_node!(waypoints[i][1:3], one(Quaternion), waypoint_shape, subvisshape, true) + end + end + Dojo.setanimation!(vis,animation) + + return_animation ? (return vis, animation) : (return vis) +end + +# ## physics functions + +function rpm_to_force_torque(::UUVWaypoint, rpm::Real, rotor_sign::Int64) + force_factor = 0.01 + torque_factor = 0.001 + + force = sign(rpm)*force_factor*rpm^2 + torque = sign(rpm)*rotor_sign*torque_factor*rpm^2 + + return [force;0;0], [torque;0;0] +end +function rpm_to_force_torque(environment::UUVWaypoint, rpms::AbstractVector, q::Quaternion) + qzpi4 = Dojo.RotZ(pi/4) + qzmpi4 = Dojo.RotZ(-pi/4) + qympi2 = Dojo.RotY(-pi/2) + orientations = [qzpi4;qzmpi4;qzmpi4;qzpi4;qympi2;qympi2] + directions = [1;1;-1;-1;1;-1] + force_vertices = [ + [0.14; -0.09; 0.059], + [0.14; 0.09; 0.059], + [-0.14; -0.09; 0.059], + [-0.14; 0.09; 0.059], + [0; -0.0855; 0.165], + [0; 0.0855; 0.165], + ] + + forces_torques = [rpm_to_force_torque(environment, rpms[i], directions[i]) for i=1:6] + forces = getindex.(forces_torques,1) + torques = getindex.(forces_torques,2) + + forces = Dojo.vector_rotate.(forces, orientations) # in local frame + torques = Dojo.vector_rotate.(torques, orientations) # in local frame + + torques_from_forces = Dojo.cross.(force_vertices, forces) + + force = Dojo.vector_rotate(sum(forces), q) # in minimal frame + torque = Dojo.vector_rotate(sum(torques .+ torques_from_forces), q) # in minimal frame + + return [force; torque] +end + +function buoyancy!(::UUVWaypoint, mechanism) + body = get_body(mechanism, :base_link) + q = body.state.q2 + force = Dojo.vector_rotate([0;0;19.5*9.81], inv(q)) # slightly positive + torque = Dojo.cross([0;0;0.2], force) + + add_external_force!(body; force, torque) + + return +end \ No newline at end of file diff --git a/DojoEnvironments/src/environments/youbot_waypoint.jl b/DojoEnvironments/src/environments/youbot_waypoint.jl new file mode 100644 index 000000000..7515c8c3f --- /dev/null +++ b/DojoEnvironments/src/environments/youbot_waypoint.jl @@ -0,0 +1,161 @@ +mutable struct YoubotWaypoint{T,N} <: Environment{T,N} + mechanism::Mechanism{T} + storage::Storage{T,N} +end + +function youbot_waypoint(; + horizon=100, + timestep=0.01, + input_scaling=timestep, + gravity=-9.81, + urdf=:youbot, + springs=0, + dampers=0, + parse_springs=true, + parse_dampers=true, + joint_limits=Dict([ + (:arm_joint_1, [-2.95,2.95]), + (:arm_joint_2, [-1.57,1.13]), + (:arm_joint_3, [-2.55,2.55]), + (:arm_joint_4, [-1.78,1.78]), + (:arm_joint_5, [-2.92,2.92]), + (:gripper_finger_joint_l, [0,0.03]), + (:gripper_finger_joint_r, [-0.03,0]), + ]), + keep_fixed_joints=false, + T=Float64) + + mechanism = get_youbot(; + timestep, + input_scaling, + gravity, + urdf, + springs, + dampers, + parse_springs, + parse_dampers, + joint_limits, + keep_fixed_joints, + T + ) + + storage = Storage(horizon, length(mechanism.bodies)) + + return YoubotWaypoint{T,horizon}(mechanism, storage) +end + +function state_map(::YoubotWaypoint, state) + xy = state[1:2] # minimal coordinates are rotated for youbot + vxy = state[4:5] # minimal coordinates are rotated for youbot + xy_minimal = Dojo.vector_rotate([xy;0],Dojo.RotZ(-pi/2))[1:2] + vxy_minimal = Dojo.vector_rotate([vxy;0],Dojo.RotZ(-pi/2))[1:2] + state[1:2] = xy_minimal + state[4:5] = vxy_minimal + + return state +end + +function input_map(environment::YoubotWaypoint, input) + # Wheels are only for visualization and not actuated + # so the wheel input must be mapped to the base + l = 0.456 + w = 0.316 + H = [ + 1 -1 -l-w + 1 1 l+w + 1 1 -l-w + 1 -1 l+w + ] + + θz = get_state(environment)[3] + + wheel_input = input[1:4] # fl, fr, bl, br + base_input = H\wheel_input/10 # incorrect but roughly ok mapping + xy = base_input[1:2] + xy_minimal = Dojo.vector_rotate([xy;0],Dojo.RotZ(θz-pi/2))[1:2] + base_input[1:2] = xy_minimal + wheel_input = zeros(4) + arm_input = input[5:11] # joint1 to joint5 to fingerl to fingerr + + input = [base_input;wheel_input;arm_input] + + return input +end + +function input_map(::YoubotWaypoint, ::Nothing) + return zeros(14) +end + +function Dojo.step!(environment::YoubotWaypoint, state, input=nothing; k=1, record=false, opts=SolverOptions()) + state = state_map(environment, state) + input = input_map(environment, input) + Dojo.step_minimal_coordinates!(environment.mechanism, state, input; opts) + record && Dojo.save_to_storage!(environment.mechanism, environment.storage, k) + + return +end + +function Dojo.simulate!(environment::YoubotWaypoint{T,N}, controller! = (environment, k) -> nothing; kwargs...) where {T,N} + mechanism = environment.mechanism + + joint_fl = get_joint(mechanism, :wheel_joint_fl) + joint_fr = get_joint(mechanism, :wheel_joint_fr) + joint_bl = get_joint(mechanism, :wheel_joint_bl) + joint_br = get_joint(mechanism, :wheel_joint_br) + + function controller_wrapper!(mechanism, k) + l = 0.456; w = 0.316; r = 0.0475 + H = [ + 1 -1 -l-w + 1 1 l+w + 1 1 -l-w + 1 -1 l+w + ] + v = get_state(environment)[4:6] + wheel_speeds = H*v/r + set_minimal_velocities!(mechanism, joint_fl, [wheel_speeds[1]]) + set_minimal_velocities!(mechanism, joint_fr, [wheel_speeds[2]]) + set_minimal_velocities!(mechanism, joint_bl, [wheel_speeds[3]]) + set_minimal_velocities!(mechanism, joint_br, [wheel_speeds[4]]) + + controller!(environment, k) + end + + simulate!(environment.mechanism, 1:N, environment.storage, controller_wrapper!; kwargs...) +end + +function get_state(environment::YoubotWaypoint) + state = get_minimal_state(environment.mechanism) + xy_minimal = state[1:2] # minimal coordinates are rotated for youbot + vxy_minimal = state[4:5] # minimal coordinates are rotated for youbot + xy = Dojo.vector_rotate([xy_minimal;0],Dojo.RotZ(pi/2))[1:2] + vxy = Dojo.vector_rotate([vxy_minimal;0],Dojo.RotZ(pi/2))[1:2] + state[1:2] = xy + state[4:5] = vxy + + return state +end + +function Dojo.visualize(environment::YoubotWaypoint; return_animation=false, kwargs...) + vis, animation = visualize(environment.mechanism, environment.storage; return_animation=true, kwargs...) + + waypoints = [ + [1;1;0.3;pi/4], + [2;0;0.3;-pi/4], + [1;-1;0.3;-3*pi/4], + [0;0;0.3;-5*pi/4], + ] + for i=1:4 + waypoint_shape = Sphere(0.2;color=RGBA(0,0.25*i,0,0.3)) + visshape = Dojo.convert_shape(waypoint_shape) + subvisshape = vis["waypoints"]["waypoint$i"] + Dojo.setobject!(subvisshape, visshape, waypoint_shape) + Dojo.atframe(animation, 1) do + Dojo.set_node!(waypoints[i][1:3], one(Quaternion), waypoint_shape, subvisshape, true) + end + end + Dojo.setanimation!(vis,animation) + + return_animation ? (return vis, animation) : (return vis) +end + diff --git a/DojoEnvironments/src/mechanisms/ant/mechanism.jl b/DojoEnvironments/src/mechanisms/ant/mechanism.jl index a3c7b07e1..fc01c8792 100644 --- a/DojoEnvironments/src/mechanisms/ant/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/ant/mechanism.jl @@ -7,7 +7,6 @@ function get_ant(; dampers=0, parse_springs=true, parse_dampers=true, - limits=true, joint_limits=Dict([ (:hip_1, [-30,30] * π / 180), (:ankle_1, [30,70] * π / 180), @@ -34,12 +33,9 @@ function get_ant(; !parse_dampers && set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/atlas/mechanism.jl b/DojoEnvironments/src/mechanisms/atlas/mechanism.jl index 7e8d5ab63..d826380d4 100644 --- a/DojoEnvironments/src/mechanisms/atlas/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/atlas/mechanism.jl @@ -7,9 +7,8 @@ function get_atlas(; dampers=0, parse_springs=true, parse_dampers=true, - limits=false, joint_limits=Dict(), - keep_fixed_joints=true, + keep_fixed_joints=false, friction_coefficient=0.8, contact_feet=true, contact_body=true, @@ -25,13 +24,10 @@ function get_atlas(; !parse_springs && set_springs!(mechanism.joints, springs) !parse_dampers && set_dampers!(mechanism.joints, dampers) - # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + # joint limits + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/block/mechanism.jl b/DojoEnvironments/src/mechanisms/block/mechanism.jl index 4dcb14076..6df7c931a 100644 --- a/DojoEnvironments/src/mechanisms/block/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/block/mechanism.jl @@ -7,9 +7,8 @@ function get_block(; color=RGBA(0.9, 0.9, 0.9, 1), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, friction_coefficient=0.8, contact=true, contact_radius=0, @@ -33,12 +32,9 @@ function get_block(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/block2d/mechanism.jl b/DojoEnvironments/src/mechanisms/block2d/mechanism.jl index a25114d6b..98be01c1e 100644 --- a/DojoEnvironments/src/mechanisms/block2d/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/block2d/mechanism.jl @@ -7,9 +7,8 @@ function get_block2d(; color=RGBA(1, 1, 0.), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, friction_coefficient=0.8, contact=true, contact_radius=0, @@ -32,12 +31,9 @@ function get_block2d(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/cartpole/mechanism.jl b/DojoEnvironments/src/mechanisms/cartpole/mechanism.jl index c3fe9c5a8..8f09e9666 100644 --- a/DojoEnvironments/src/mechanisms/cartpole/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/cartpole/mechanism.jl @@ -4,26 +4,25 @@ function get_cartpole(; gravity=-9.81, slider_mass=1, pendulum_mass=1, - pendulum_length=1, + link_length=1, radius=0.075, color=RGBA(0.7, 0.7, 0.7, 1), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, T=Float64) # mechanism origin = Origin{Float64}() slider = Capsule(1.5 * radius, 1, slider_mass; orientation_offset=Dojo.RotX(0.5 * π), color, name=:cart) - pendulum = Capsule(radius, pendulum_length, pendulum_mass; color, name=:pole) + pendulum = Capsule(radius, link_length, pendulum_mass; color, name=:pole) bodies = [slider, pendulum] joint_origin_slider = JointConstraint(Prismatic(origin, slider, Y_AXIS); name=:cart_joint) joint_slider_pendulum = JointConstraint(Revolute(slider, pendulum, X_AXIS; - child_vertex=-0.5*pendulum_length*Z_AXIS); name=:pole_joint) + child_vertex=-0.5*link_length*Z_AXIS); name=:pole_joint) joints = [joint_origin_slider, joint_slider_pendulum] mechanism = Mechanism(origin, bodies, joints; @@ -34,12 +33,9 @@ function get_cartpole(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero configuration initialize_cartpole!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/dzhanibekov/mechanism.jl b/DojoEnvironments/src/mechanisms/dzhanibekov/mechanism.jl index 0d62d17ad..5b122db87 100644 --- a/DojoEnvironments/src/mechanisms/dzhanibekov/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/dzhanibekov/mechanism.jl @@ -5,9 +5,8 @@ function get_dzhanibekov(; springs=0, dampers=0, color=RGBA(0.9,0.9,0.9,1), - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, T=Float64) # mechanism @@ -32,12 +31,9 @@ function get_dzhanibekov(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero configuration initialize_dzhanibekov!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/exoskeleton/mechanism.jl b/DojoEnvironments/src/mechanisms/exoskeleton/mechanism.jl index 4d7975614..bfe5df20b 100644 --- a/DojoEnvironments/src/mechanisms/exoskeleton/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/exoskeleton/mechanism.jl @@ -7,7 +7,6 @@ function get_exoskeleton(; dampers=0, parse_springs=true, parse_dampers=true, - limits=true, joint_limits=Dict([ (:sAA, [0, 90]*π/180), (:sFE, [0, 90]*π/180), @@ -27,12 +26,9 @@ function get_exoskeleton(; !parse_dampers && set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero configuration initialize_exoskeleton!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/fourbar/mechanism.jl b/DojoEnvironments/src/mechanisms/fourbar/mechanism.jl index 86a15ae0b..0b7dba0ce 100644 --- a/DojoEnvironments/src/mechanisms/fourbar/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/fourbar/mechanism.jl @@ -7,7 +7,6 @@ function get_fourbar(; dampers=0, parse_springs=true, parse_dampers=true, - limits=false, joint_limits=Dict(), keep_fixed_joints=false, T=Float64) @@ -22,13 +21,10 @@ function get_fourbar(; !parse_springs && set_springs!(mechanism.joints, springs) !parse_dampers && set_dampers!(mechanism.joints, dampers) - # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + # joint limits + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero configuration initialize_fourbar!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl b/DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl index 772b3a479..61f55b3ce 100644 --- a/DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/halfcheetah/mechanism.jl @@ -7,7 +7,6 @@ function get_halfcheetah(; dampers=0, parse_springs=true, parse_dampers=true, - limits=true, joint_limits=Dict([ (:bthigh, [-0.52,1.05]), (:bshin, [-0.785,0.785]), @@ -15,7 +14,7 @@ function get_halfcheetah(; (:fthigh, [-1,0.7]), (:fshin, [-1.20,0.87]), (:ffoot, [-0.5,0.5])]), - keep_fixed_joints=true, + keep_fixed_joints=false, friction_coefficient=0.4, contact_feet=true, contact_body=true, @@ -31,13 +30,10 @@ function get_halfcheetah(; !parse_springs && set_springs!(mechanism.joints, springs) !parse_dampers && set_dampers!(mechanism.joints, dampers) - # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + # joint limits + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/hopper/mechanism.jl b/DojoEnvironments/src/mechanisms/hopper/mechanism.jl index 5833aedf7..2d628497c 100644 --- a/DojoEnvironments/src/mechanisms/hopper/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/hopper/mechanism.jl @@ -7,12 +7,11 @@ function get_hopper(; dampers=0, parse_springs=true, parse_dampers=true, - limits=false, joint_limits=Dict([ (:thigh, [0,150] * π / 180), (:leg, [0,150] * π / 180), (:foot, [-45,45] * π / 180)]), - keep_fixed_joints=true, + keep_fixed_joints=false, friction_coefficient=2, contact_foot=true, contact_body=true, @@ -29,12 +28,9 @@ function get_hopper(; !parse_dampers && set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/humanoid/mechanism.jl b/DojoEnvironments/src/mechanisms/humanoid/mechanism.jl index 5bc07dc26..48a6d35ed 100644 --- a/DojoEnvironments/src/mechanisms/humanoid/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/humanoid/mechanism.jl @@ -7,9 +7,8 @@ function get_humanoid(; dampers=0, parse_springs=true, parse_dampers=true, - limits=false, joint_limits=Dict(), - keep_fixed_joints=true, + keep_fixed_joints=false, friction_coefficient=0.8, contact_feet=true, T=Float64) @@ -24,13 +23,10 @@ function get_humanoid(; !parse_springs && set_springs!(mechanism.joints, springs) !parse_dampers && set_dampers!(mechanism.joints, dampers) - # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + # joint limits + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/npendulum/mechanism.jl b/DojoEnvironments/src/mechanisms/npendulum/mechanism.jl index e8868685e..57acbfd8b 100644 --- a/DojoEnvironments/src/mechanisms/npendulum/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/npendulum/mechanism.jl @@ -4,13 +4,12 @@ function get_npendulum(; gravity=-9.81, num_bodies=5, mass=1, - length=1, + link_length=1, color=RGBA(1, 0, 0), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, base_joint_type=:Revolute, rest_joint_type=:Revolute, T=Float64) @@ -18,16 +17,16 @@ function get_npendulum(; # mechanism origin = Origin{T}() - bodies = [Box(0.05, 0.05, length, mass; color) for i = 1:num_bodies] + bodies = [Box(0.05, 0.05, link_length, mass; color) for i = 1:num_bodies] jointb1 = JointConstraint(Dojo.Prototype(base_joint_type, origin, bodies[1], X_AXIS; - parent_vertex=Z_AXIS*num_bodies + [0;0;0.2], child_vertex=Z_AXIS*length/2)) + parent_vertex=(link_length+0.1)*Z_AXIS*num_bodies, child_vertex=Z_AXIS*link_length/2)) joints = JointConstraint{T}[ jointb1; [ JointConstraint(Dojo.Prototype(rest_joint_type, bodies[i - 1], bodies[i], X_AXIS; - parent_vertex=-Z_AXIS*length/2, child_vertex=Z_AXIS*length/2)) for i = 2:num_bodies + parent_vertex=-Z_AXIS*link_length/2, child_vertex=Z_AXIS*link_length/2)) for i = 2:num_bodies ] ] @@ -39,12 +38,9 @@ function get_npendulum(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero configuration initialize_npendulum!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/nslider/mechanism.jl b/DojoEnvironments/src/mechanisms/nslider/mechanism.jl index e638e6aff..d72d89451 100644 --- a/DojoEnvironments/src/mechanisms/nslider/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/nslider/mechanism.jl @@ -6,9 +6,8 @@ function get_nslider(; color=RGBA(1, 0, 0), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, T=Float64) # mechanism @@ -32,12 +31,9 @@ function get_nslider(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero coordinates initialize_nslider!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/panda/mechanism.jl b/DojoEnvironments/src/mechanisms/panda/mechanism.jl index 20b4caee1..8aae97294 100644 --- a/DojoEnvironments/src/mechanisms/panda/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/panda/mechanism.jl @@ -7,7 +7,6 @@ function get_panda(; dampers=5, parse_springs=true, parse_dampers=false, - limits=true, joint_limits=Dict([ (:joint1, [-2.8973, 2.8973]), (:joint2, [-1.7628, 1.7628]), @@ -32,12 +31,9 @@ function get_panda(; !parse_dampers && set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero configuration initialize_panda!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/pendulum/mechanism.jl b/DojoEnvironments/src/mechanisms/pendulum/mechanism.jl index cf0a5f65d..08f7248fd 100644 --- a/DojoEnvironments/src/mechanisms/pendulum/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/pendulum/mechanism.jl @@ -3,11 +3,10 @@ function get_pendulum(; input_scaling=timestep, gravity=-9.81, mass=1, - length=1, + link_length=1, color=RGBA(1, 0, 0), springs=0, dampers=0, - limits=false, joint_limits=Dict(), spring_offset=szeros(1), orientation_offset=one(Quaternion), @@ -16,11 +15,11 @@ function get_pendulum(; # mechanism origin = Origin{T}() - body = Box(0.1, 0.1, length, mass; color, name=:pendulum) + body = Box(0.1, 0.1, link_length, mass; color, name=:pendulum) bodies = [body] joint = JointConstraint(Revolute(origin, body, X_AXIS; - parent_vertex=1.1*Z_AXIS, child_vertex=0.5*Z_AXIS, + parent_vertex=(link_length+0.1)*Z_AXIS, child_vertex=0.5*link_length*Z_AXIS, rot_spring_offset=spring_offset, orientation_offset), name=:joint) joints = [joint] @@ -32,13 +31,10 @@ function get_pendulum(; set_springs!(mechanism.joints, springs) set_dampers!(mechanism.joints, dampers) - # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + # joint limits + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero configuration initialize_pendulum!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl b/DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl index be5c8d223..d637c88c4 100644 --- a/DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/quadrotor/mechanism.jl @@ -7,7 +7,6 @@ function get_quadrotor(; dampers=0, parse_springs=true, parse_dampers=true, - limits=true, joint_limits=Dict(), keep_fixed_joints=false, friction_coefficient=0.5, @@ -26,12 +25,9 @@ function get_quadrotor(; !parse_dampers && set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/quadruped/mechanism.jl b/DojoEnvironments/src/mechanisms/quadruped/mechanism.jl index 26d8ebe68..d889b529f 100644 --- a/DojoEnvironments/src/mechanisms/quadruped/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/quadruped/mechanism.jl @@ -8,13 +8,12 @@ function get_quadruped(; parse_springs=true, parse_dampers=true, spring_offset=true, - limits=true, joint_limits=Dict(vcat([[ (Symbol(group,:_hip_joint), [-0.5,0.5]), (Symbol(group,:_thigh_joint), [-0.5,1.5]), (Symbol(group,:_calf_joint), [-2.5,-1])] for group in [:FR, :FL, :RR, :RL]]...)), - keep_fixed_joints=true, + keep_fixed_joints=false, friction_coefficient=0.8, contact_feet=true, contact_body=true, @@ -49,13 +48,10 @@ function get_quadruped(; get_node(mechanism, :RL_calf_joint).rotational.spring_offset=θ_calf*sones(1) end - # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + # joint limits + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl b/DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl index b10ed5db9..cfadac2f9 100644 --- a/DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/raiberthopper/mechanism.jl @@ -9,9 +9,8 @@ function get_raiberthopper(; color=RGBA(0.9, 0.9, 0.9), springs=[0;0], dampers=[0;0.1], - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, friction_coefficient=0.5, contact_foot=true, contact_body=true, @@ -37,12 +36,9 @@ function get_raiberthopper(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/slider/mechanism.jl b/DojoEnvironments/src/mechanisms/slider/mechanism.jl index f7438e7fb..a6d0c8a73 100644 --- a/DojoEnvironments/src/mechanisms/slider/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/slider/mechanism.jl @@ -5,9 +5,8 @@ function get_slider(; color=RGBA(1, 0, 0), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, T=Float64) # mechanism @@ -28,12 +27,9 @@ function get_slider(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero coordinates initialize_slider!(mechanism) diff --git a/DojoEnvironments/src/mechanisms/snake/mechanism.jl b/DojoEnvironments/src/mechanisms/snake/mechanism.jl index 7a670d6ae..3d8fcc9f5 100644 --- a/DojoEnvironments/src/mechanisms/snake/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/snake/mechanism.jl @@ -3,15 +3,14 @@ function get_snake(; input_scaling=timestep, gravity=-9.81, num_bodies=2, - length=1, + link_length=1, radius=0.05, color=RGBA(0.9, 0.9, 0.9), springs=0, dampers=0, - limits=false, joint_limits=Dict(), joint_type=:Spherical, - keep_fixed_joints=false, + keep_fixed_joints=true, friction_coefficient=0.8, contact=true, contact_type=:nonlinear, @@ -20,7 +19,7 @@ function get_snake(; # mechanism origin = Origin{T}() - bodies = [Box(length, 3 * radius, 2 * radius, length; color) for i = 1:num_bodies] + bodies = [Box(link_length, 3 * radius, 2 * radius, link_length; color) for i = 1:num_bodies] jointb1 = JointConstraint(Floating(origin, bodies[1])) @@ -28,7 +27,7 @@ function get_snake(; jointb1; [ JointConstraint(Dojo.Prototype(joint_type, bodies[i - 1], bodies[i], X_AXIS; - parent_vertex=-X_AXIS*length/2, child_vertex=X_AXIS*length/2)) for i = 2:num_bodies + parent_vertex=-X_AXIS*link_length/2, child_vertex=X_AXIS*link_length/2)) for i = 2:num_bodies ] ] @@ -40,24 +39,21 @@ function get_snake(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] if contact contact_bodies = [bodies;bodies] # we need to duplicate contacts for prismatic joint for instance - n = Base.length(contact_bodies) + n = length(contact_bodies) normals = fill(Z_AXIS,n) friction_coefficients = fill(friction_coefficient,n) contact_origins = [ - fill(X_AXIS*length/2, Int64(n/2)) - fill(-X_AXIS*length/2, Int64(n/2)) + fill(X_AXIS*link_length/2, Int64(n/2)) + fill(-X_AXIS*link_length/2, Int64(n/2)) ] contacts = [contacts;contact_constraint(contact_bodies, normals; friction_coefficients, contact_origins, contact_type)] end diff --git a/DojoEnvironments/src/mechanisms/sphere/mechanism.jl b/DojoEnvironments/src/mechanisms/sphere/mechanism.jl index b17abf090..a5e07bd53 100644 --- a/DojoEnvironments/src/mechanisms/sphere/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/sphere/mechanism.jl @@ -7,9 +7,8 @@ function get_sphere(; color=RGBA(0.9, 0.9, 0.9), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, friction_coefficient=0.8, contact=true, contact_type=:nonlinear, @@ -29,12 +28,9 @@ function get_sphere(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/tippetop/mechanism.jl b/DojoEnvironments/src/mechanisms/tippetop/mechanism.jl index 4dee2bb8e..1ddbc00c9 100644 --- a/DojoEnvironments/src/mechanisms/tippetop/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/tippetop/mechanism.jl @@ -8,9 +8,8 @@ function get_tippetop(; color=RGBA(0.9, 0.9, 0.9, 1.0), springs=0, dampers=0, - limits=false, joint_limits=Dict(), - keep_fixed_joints=false, + keep_fixed_joints=true, friction_coefficient=0.4, contact=true, contact_type=:nonlinear, @@ -39,12 +38,9 @@ function get_tippetop(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/twister/mechanism.jl b/DojoEnvironments/src/mechanisms/twister/mechanism.jl index ad7d68047..7aa67f03f 100644 --- a/DojoEnvironments/src/mechanisms/twister/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/twister/mechanism.jl @@ -8,10 +8,9 @@ function get_twister(; color=RGBA(1, 0, 0), springs=0, dampers=0, - limits=false, joint_limits=Dict(), joint_type=:Prismatic, - keep_fixed_joints=false, + keep_fixed_joints=true, friction_coefficient=0.8, contact=true, contact_type=:nonlinear, @@ -41,12 +40,9 @@ function get_twister(; set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga.urdf b/DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga.urdf index 6c80467d8..efbdd3d11 100644 --- a/DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga.urdf +++ b/DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga.urdf @@ -1,6 +1,23 @@ + + + + + + + + + + + + + + diff --git a/DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga_fixed_rotors.urdf b/DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga_fixed_rotors.urdf index b939a45b0..d476bac6b 100644 --- a/DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga_fixed_rotors.urdf +++ b/DojoEnvironments/src/mechanisms/uuv/dependencies/mini_tortuga_fixed_rotors.urdf @@ -1,6 +1,23 @@ + + + + + + + + + + + + + + diff --git a/DojoEnvironments/src/mechanisms/uuv/mechanism.jl b/DojoEnvironments/src/mechanisms/uuv/mechanism.jl index 79ed6ef72..2270cd8d8 100644 --- a/DojoEnvironments/src/mechanisms/uuv/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/uuv/mechanism.jl @@ -4,10 +4,9 @@ function get_uuv(; gravity=0, urdf=:mini_tortuga_fixed_rotors, springs=0, - dampers=[20], + dampers=0, parse_springs=true, parse_dampers=true, - limits=true, joint_limits=Dict(), keep_fixed_joints=false, friction_coefficient=0.5, @@ -16,7 +15,7 @@ function get_uuv(; # mechanism path = joinpath(@__DIR__, "dependencies/$(string(urdf)).urdf") - mechanism = Mechanism(path; floating=true, T, + mechanism = Mechanism(path; floating=false, T, gravity, timestep, input_scaling, parse_dampers, keep_fixed_joints) @@ -25,12 +24,9 @@ function get_uuv(; !parse_dampers && set_dampers!(mechanism.joints, dampers) # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] @@ -40,7 +36,7 @@ function get_uuv(; body_in_contact = get_body(mechanism, :base_link) n = 2 normals = fill(Z_AXIS,n) - friction_coefficients = fill(0.5,n) + friction_coefficients = fill(friction_coefficient,n) contact_radii = fill(0.21,n) contact_origins = [ [0.12; 0; 0.07], diff --git a/DojoEnvironments/src/mechanisms/walker/mechanism.jl b/DojoEnvironments/src/mechanisms/walker/mechanism.jl index 83cc5e5a0..2bb3c53a5 100644 --- a/DojoEnvironments/src/mechanisms/walker/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/walker/mechanism.jl @@ -7,7 +7,6 @@ function get_walker(; dampers=0, parse_springs=true, parse_dampers=true, - limits=true, joint_limits=Dict([ (:thigh, [0,150] * π / 180), (:leg, [0,150] * π / 180), @@ -31,13 +30,10 @@ function get_walker(; !parse_springs && set_springs!(mechanism.joints, springs) !parse_dampers && set_dampers!(mechanism.joints, dampers) - # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + # joint limits + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # contacts contacts = ContactConstraint{T}[] diff --git a/DojoEnvironments/src/mechanisms/youbot/dependencies/youbot.urdf b/DojoEnvironments/src/mechanisms/youbot/dependencies/youbot.urdf index e2d37185e..30b5f8215 100644 --- a/DojoEnvironments/src/mechanisms/youbot/dependencies/youbot.urdf +++ b/DojoEnvironments/src/mechanisms/youbot/dependencies/youbot.urdf @@ -288,6 +288,7 @@ + @@ -313,7 +314,7 @@ - + @@ -343,7 +344,6 @@ - @@ -353,7 +353,7 @@ - + @@ -385,7 +385,7 @@ - + @@ -417,7 +417,7 @@ - + @@ -450,7 +450,7 @@ - + diff --git a/DojoEnvironments/src/mechanisms/youbot/mechanism.jl b/DojoEnvironments/src/mechanisms/youbot/mechanism.jl index 18bfd3c0a..e4dd237f1 100644 --- a/DojoEnvironments/src/mechanisms/youbot/mechanism.jl +++ b/DojoEnvironments/src/mechanisms/youbot/mechanism.jl @@ -7,8 +7,15 @@ function get_youbot(; dampers=0, parse_springs=true, parse_dampers=true, - limits=false, - joint_limits=Dict(), + joint_limits=Dict([ + (:arm_joint_1, [-2.95,2.95]), + (:arm_joint_2, [-1.57,1.13]), + (:arm_joint_3, [-2.55,2.55]), + (:arm_joint_4, [-1.78,1.78]), + (:arm_joint_5, [-2.92,2.92]), + # (:gripper_finger_joint_l, [0,0.03]), + # (:gripper_finger_joint_r, [-0.03,0]), + ]), keep_fixed_joints=false, T=Float64) @@ -22,13 +29,10 @@ function get_youbot(; !parse_springs && set_springs!(mechanism.joints, springs) !parse_dampers && set_dampers!(mechanism.joints, dampers) - # joint limits - if limits - joints = set_limits(mechanism, joint_limits) - - mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; - gravity, timestep, input_scaling) - end + # joint limits + joints = set_limits(mechanism, joint_limits) + mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints; + gravity, timestep, input_scaling) # zero configuration initialize_youbot!(mechanism) @@ -37,10 +41,17 @@ function get_youbot(; return mechanism end -function initialize_youbot!(mechanism) +function initialize_youbot!(mechanism; + body_position=zeros(2), body_orientation=0, arm_angles=zeros(5)) zero_velocities!(mechanism) zero_coordinates!(mechanism) + set_minimal_coordinates!(mechanism, get_joint(mechanism, :base_footprint_joint), [body_position;body_orientation]) + set_minimal_coordinates!(mechanism, get_joint(mechanism, :arm_joint_1), [arm_angles[1]]) + set_minimal_coordinates!(mechanism, get_joint(mechanism, :arm_joint_2), [arm_angles[2]]) + set_minimal_coordinates!(mechanism, get_joint(mechanism, :arm_joint_3), [arm_angles[3]]) + set_minimal_coordinates!(mechanism, get_joint(mechanism, :arm_joint_4), [arm_angles[4]]) + set_minimal_coordinates!(mechanism, get_joint(mechanism, :arm_joint_5), [arm_angles[5]]) return end \ No newline at end of file diff --git a/DojoEnvironments/test/environments.jl b/DojoEnvironments/test/environments.jl index 4cf1acec3..f16fcfc72 100644 --- a/DojoEnvironments/test/environments.jl +++ b/DojoEnvironments/test/environments.jl @@ -2,13 +2,18 @@ environments = [ :ant_ars, :cartpole_dqn, :pendulum, + :quadruped_waypoint, :quadruped_sampling, + :quadrotor_waypoint, + :uuv_waypoint, + :youbot_waypoint, ] for name in environments - env = get_environment(name; horizon=2) + env = get_environment(name; horizon=2) x = get_state(env) u = DojoEnvironments.input_map(env, nothing) + set_input!(env, nothing) step!(env, x) step!(env, x, u) simulate!(env; record=true) diff --git a/Project.toml b/Project.toml index 6a1e332b0..1edf25a20 100644 --- a/Project.toml +++ b/Project.toml @@ -16,14 +16,12 @@ LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" Meshing = "e6723b4c-ebff-59f1-b4b7-d97aa5274f73" -Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" Polyhedra = "67491407-f73d-577b-9b50-8179a7c68029" Quaternions = "94ee1d12-ae83-5a48-8b1c-48b8ff168ae0" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" -Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" [compat] Colors = "<0.12.10, 0.12.10" diff --git a/README.md b/README.md index d8284048c..54895cddf 100644 --- a/README.md +++ b/README.md @@ -30,8 +30,9 @@ A differentiable physics engine for robotics ## Learning and Control

- - + + +

## System Identification diff --git a/docs/src/api.md b/docs/src/api.md index d460b8e03..334536dd1 100644 --- a/docs/src/api.md +++ b/docs/src/api.md @@ -40,6 +40,8 @@ zero_coordinates! zero_velocities! root_to_leaves_ordering set_floating_base +set_external_force! +add_external_force! ``` ### Nodes @@ -144,7 +146,6 @@ mehrotra! ```@docs visualize build_robot -set_robot set_camera! set_light! set_surface! diff --git a/docs/src/assets/animations/quadrotor.gif b/docs/src/assets/animations/quadrotor.gif new file mode 100644 index 000000000..07f935281 Binary files /dev/null and b/docs/src/assets/animations/quadrotor.gif differ diff --git a/examples/Project.toml b/examples/Project.toml index 4dbf8a28f..a4b336bd8 100644 --- a/examples/Project.toml +++ b/examples/Project.toml @@ -16,7 +16,7 @@ Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" [compat] ClosedIntervals = "<0.3.0, 0.3.0" -ControlSystemsBase = "1.0, 1.7" +ControlSystemsBase = "1.0, 1.4" Dojo = "<0.6.1, 0.6.1" DojoEnvironments = "<0.4.1, 0.4.1" Flux = "<0.13.14, 0.13.14" diff --git a/examples/control/quadrotor_cascade.jl b/examples/control/quadrotor_cascade.jl new file mode 100644 index 000000000..1eaebf73c --- /dev/null +++ b/examples/control/quadrotor_cascade.jl @@ -0,0 +1,37 @@ +# ### Setup +# PKG_SETUP +using Dojo +using DojoEnvironments +using LinearAlgebra + +# ### Environment +quadrotor_env = get_environment(:quadrotor_waypoint; horizon=200) + +# ### Controllers +trans_z_mode = normalize([1;1;1;1]) + +function velocity_controller!(environment, v_des) + v_is = get_state(environment)[9] + error_z = v_des - v_is + thrust = (10*error_z - 1*v_is + 5.1)*trans_z_mode # P, D, feedforward + + rpm = thrust*20 + set_input!(environment, rpm) +end + +function position_controller!(environment, z_des) + z_is = get_state(environment)[3] + v_des = z_des - z_is + velocity_controller!(environment, v_des) +end + +function controller!(environment, k) + position_controller!(environment, 0.3) +end + +# ### Simulate +simulate!(quadrotor_env, controller!; record=true) + +# ### Visualize +vis = visualize(quadrotor_env) +render(vis) diff --git a/examples/learning/ant_ars.jl b/examples/learning/ant_ars.jl index 447d4b186..2235d8c52 100644 --- a/examples/learning/ant_ars.jl +++ b/examples/learning/ant_ars.jl @@ -71,7 +71,7 @@ env = get_environment(:ant_ars; # ### Reset and rollout functions function reset_state!(env) - initialize!(env.mechanism, :ant) + initialize!(env, :ant) return end @@ -207,14 +207,14 @@ policies_best = (policies[max_idx])[1]; input_size = 37 normalizer = Normalizer(input_size) -function controller!(mechanism, k) - state = DojoEnvironments.get_state(env) +function controller!(environment, k) + state = get_state(env) observe!(normalizer, state) state = normalize(normalizer, state) action = θ * state - set_input!(mechanism, DojoEnvironments.input_map(env,action)) + set_input!(environment, action) end # ### Visualize policy diff --git a/examples/learning/cartpole_dqn.jl b/examples/learning/cartpole_dqn.jl index 950987d3e..197c6f9ba 100644 --- a/examples/learning/cartpole_dqn.jl +++ b/examples/learning/cartpole_dqn.jl @@ -103,13 +103,13 @@ function CartPoleEnv(; T=Float64, record=false, continuous=false, rng=Random.GLO params = CartPoleEnvParams{T}(; kwargs...) ## create Dojo Environment - dojo_env = DojoEnvironments.get_environment(:cartpole_dqn; + dojo_env = get_environment(:cartpole_dqn; horizon=params.max_steps+1, timestep = params.dt, gravity = -params.gravity, slider_mass = params.masscart, pendulum_mass = params.masspole, - pendulum_length = params.halflength*2 + link_length = params.halflength*2 ) env = CartPoleEnv(dojo_env, record, params, zeros(T, 4), continuous ? zero(T) : zero(Int), false, 0, rng) diff --git a/examples/learning/quadruped_sampling.jl b/examples/learning/quadruped_sampling.jl index cefdb3d51..a1a8f1f79 100644 --- a/examples/learning/quadruped_sampling.jl +++ b/examples/learning/quadruped_sampling.jl @@ -17,7 +17,7 @@ explore_factor = 0.1 distancestorage = zeros(M); # ### Environment -env = get_environment(:quadruped_sampling; horizon=N, timestep=0.001, limits=false, gravity=-9.81, contact_body=false) +env = get_environment(:quadruped_sampling; horizon=N, timestep=0.001, joint_limits=Dict(), gravity=-9.81, contact_body=false) # ### Controller legmovement(k,a,b,c,offset) = a*cos(k*b*0.01*2*pi+offset)+c @@ -56,23 +56,23 @@ end # ### Reset and rollout functions function reset_state!(env) - initialize!(env.mechanism, :quadruped; body_position=[0;0;-0.43], hip_angle=0, thigh_angle=paramcontainer[1][3], calf_angle=paramcontainer[1][5]) + initialize!(env, :quadruped; body_position=[0;0;-0.43], hip_angle=0, thigh_angle=paramcontainer[1][3], calf_angle=paramcontainer[1][5]) calf_state = get_body(env.mechanism, :FR_calf).state position = get_sdf(get_contact(env.mechanism, :FR_calf_contact), Dojo.current_position(calf_state), Dojo.current_orientation(calf_state)) - initialize!(env.mechanism, :quadruped; body_position=[0;0;-position-0.43], hip_angle=0, thigh_angle=paramcontainer[1][3], calf_angle=paramcontainer[1][5]) + initialize!(env, :quadruped; body_position=[0;0;-position-0.43], hip_angle=0, thigh_angle=paramcontainer[1][3], calf_angle=paramcontainer[1][5]) end function rollout(env; record=false) for k=1:N - x = DojoEnvironments.get_state(env) + x = get_state(env) if x[3] < 0 || !all(isfinite.(x)) || abs(x[1]) > 1000 # upsidedown || failed || "exploding" println(" failed") return 1 end u = controller!(x, k) - DojoEnvironments.step!(env, x, u; k, record) + step!(env, x, u; k, record) end return 0 end @@ -123,11 +123,11 @@ end # ### Controller for best parameter set paramcontainer[1] = paramstorage[end] -function environment_controller!(mechanism, k) - x = DojoEnvironments.get_state(env) +function environment_controller!(environment, k) + x = get_state(env) u = controller!(x, k) - set_input!(mechanism, DojoEnvironments.input_map(env,u)) + set_input!(environment, u) end # ### Visualize learned behavior diff --git a/examples/simulation/all_mechanisms.jl b/examples/simulation/all_mechanisms.jl index a2e2ece11..7329e660a 100644 --- a/examples/simulation/all_mechanisms.jl +++ b/examples/simulation/all_mechanisms.jl @@ -11,6 +11,7 @@ mechanisms = [ :block2d, :cartpole, :dzhanibekov, + :exoskeleton, :fourbar, :halfcheetah, :hopper, @@ -19,6 +20,7 @@ mechanisms = [ :nslider, :panda, :pendulum, + :quadrotor, :quadruped, :raiberthopper, :slider, @@ -26,6 +28,7 @@ mechanisms = [ :sphere, :tippetop, :twister, + :uuv, :walker, :youbot, ] @@ -33,10 +36,10 @@ mechanisms = [ # ### Select mechanism name = :ant -# ### Get mechanism (check DojoEnvironment/mechanisms files for kwargs) +# ### Get mechanism (check DojoEnvironments/src/mechanisms files for kwargs) mech = get_mechanism(name) -# ### Initialize mechanism (check DojoEnvironment/mechanisms files for kwargs) +# ### Initialize mechanism (check DojoEnvironments/src/mechanisms files for kwargs) initialize!(mech, name) # ### Simulate mechanism diff --git a/examples/simulation/pendulum_direct.jl b/examples/simulation/pendulum_direct.jl index 002b9889a..293e01b60 100644 --- a/examples/simulation/pendulum_direct.jl +++ b/examples/simulation/pendulum_direct.jl @@ -4,14 +4,14 @@ using Dojo # ### Parameters radius = 0.1 -length = 1 +link_length = 1 mass = 1 rotation_axis = [1;0;0] -connection = [0;0;length/2] +connection = [0;0;link_length/2] # ### Mechanism components origin = Origin() -body = Cylinder(radius, length, mass) +body = Cylinder(radius, link_length, mass) joint = JointConstraint(Revolute(origin, body, rotation_axis; child_vertex=connection)) # ### Construct Mechanism diff --git a/examples/simulation/pendulum_mechanism.jl b/examples/simulation/pendulum_mechanism.jl index eeb410e21..cd4005e2f 100644 --- a/examples/simulation/pendulum_mechanism.jl +++ b/examples/simulation/pendulum_mechanism.jl @@ -4,7 +4,7 @@ using Dojo using DojoEnvironments # ### Get mechanism (check DojoEnvironment/mechanisms files for kwargs) -mechanism = get_mechanism(:pendulum; timestep=0.02, length=0.75) +mechanism = get_mechanism(:pendulum; timestep=0.02, link_length=0.75) # ### Initialize mechanism (check DojoEnvironment/mechanisms files for kwargs) initialize!(mechanism, :pendulum; angle=-0.3) diff --git a/examples/system_identification/data/datasets/synthetic_sphere.jld2 b/examples/system_identification/data/datasets/synthetic_sphere.jld2 index 76602d0f0..c3ed7ab26 100644 Binary files a/examples/system_identification/data/datasets/synthetic_sphere.jld2 and b/examples/system_identification/data/datasets/synthetic_sphere.jld2 differ diff --git a/src/Dojo.jl b/src/Dojo.jl index 37de94471..4561d6995 100644 --- a/src/Dojo.jl +++ b/src/Dojo.jl @@ -157,7 +157,8 @@ export CombinedShapes, get_body, get_node, - set_external_force! + set_external_force!, + add_external_force! # Joints export diff --git a/src/bodies/set.jl b/src/bodies/set.jl index b24017431..d80817648 100644 --- a/src/bodies/set.jl +++ b/src/bodies/set.jl @@ -114,6 +114,22 @@ function set_external_force!(body::Body; force=zeros(3), torque=zeros(3), vertex return end +""" + add_external_force!(body; force, torque, vertex) + + adds an additional external force on a body + + body: Body + force: force in body frame + torque: torque in local frame +""" +function add_external_force!(body::Body; force=zeros(3), torque=zeros(3), vertex=zeros(3)) + body.state.Fext += vector_rotate(force, body.state.q2) + body.state.τext += torque + cross(vertex, force) + + return +end + function clear_external_force!(body::Body{T}) where T body.state.Fext = szeros(T,3) body.state.τext = szeros(T,3) diff --git a/src/visuals/visualizer.jl b/src/visuals/visualizer.jl index deec55733..97977d05b 100644 --- a/src/visuals/visualizer.jl +++ b/src/visuals/visualizer.jl @@ -215,93 +215,6 @@ function build_robot(mechanism::Mechanism; return vis end -""" - set_robot(vis, mechanism, z; show_contact, name) - - visualze mechanism configuration from maximal representation - - vis: Visualizer - mechanism: Mechanism - z: maximal state - show_contact: flag to show contact locations on mechanism - name: unique identifier -""" -function set_robot(vis::Visualizer, mechanism::Mechanism, z::Vector{T}; - show_joint::Bool=false, - joint_radius=0.1, - show_contact::Bool=true, - name::Symbol=:robot) where {T} - - (length(z) == minimal_dimension(mechanism)) && (z = minimal_to_maximal(mechanism, z)) - bodies = mechanism.bodies - origin = mechanism.origin - - # Bodies and Contacts - for (id, body) in enumerate(bodies) - x, _, q, _ = unpack_maximal_state(z, id) - shape = body.shape - visshape = convert_shape(shape) - subvisshape = nothing - showshape = false - if visshape !== nothing - subvisshape = vis[name][:bodies][Symbol(body.name, "__id_$id")] - showshape = true - end - - set_node!(x, q, id, shape, subvisshape, showshape) - - if show_joint - for (jd, joint) in enumerate(mechanism.joints) - if joint.child_id == body.id - radius = joint_radius - joint_shape = Sphere(radius, - position_offset=joint.translational.vertices[2], - color=RGBA(0.0, 0.0, 1.0, 0.5)) - visshape = convert_shape(joint_shape) - subvisshape = nothing - showshape = false - if visshape !== nothing - subvisshape = vis[name][:joints][Symbol(joint.name, "__id_$(jd)")] - showshape = true - end - set_node!(x, q, id, joint_shape, subvisshape, showshape) - end - end - end - - if show_contact - for (jd, contact) in enumerate(mechanism.contacts) - if contact.parent_id == body.id - radius = abs(contact.model.collision.contact_radius) - (radius == 0.0) && (radius = 0.01) - contact_shape = Sphere(radius, - position_offset=(contact.model.collision.contact_origin), - orientation_offset=one(Quaternion), color=RGBA(1.0, 0.0, 0.0, 0.5)) - visshape = convert_shape(contact_shape) - subvisshape = nothing - showshape = false - if visshape !== nothing - subvisshape = vis[name][:contacts][Symbol(contact.name, "__id_$(jd)")] - showshape = true - end - set_node!(x, q, id, contact_shape, subvisshape, showshape) - end - end - end - end - - # Origin - id = origin.id - shape = origin.shape - visshape = convert_shape(shape) - if visshape !== nothing - subvisshape = vis[name][:bodies][Symbol(:origin, "_id")] - shapetransform = transform(szeros(T,3), one(Quaternion{T}), shape) - settransform!(subvisshape, shapetransform) - end - return vis -end - function transform(x, q, shape) scale_transform = MeshCat.LinearMap(diagm(shape.scale)) x_transform = MeshCat.Translation(x + vector_rotate(shape.position_offset, q)) @@ -312,7 +225,7 @@ end MeshCat.js_scaling(s::AbstractVector) = s MeshCat.js_position(p::AbstractVector) = p -function set_node!(x, q, id, shape, shapevisualizer, showshape) +function set_node!(x, q, shape, shapevisualizer, showshape) if showshape # TODO currently setting props directly because MeshCat/Rotations doesn't convert scaled rotation properly. # If this changes, do similarily to origin @@ -329,7 +242,7 @@ function animate_node!(storage::Storage{T,N}, id, shape, animation, shapevisuali x = storage.x[id][i] q = storage.q[id][i] atframe(animation, frame_id) do - set_node!(x, q, id, shape, shapevisualizer, showshape) + set_node!(x, q, shape, shapevisualizer, showshape) end frame_id += 1 end diff --git a/test/data.jl b/test/data.jl index 229469b91..40decd9aa 100644 --- a/test/data.jl +++ b/test/data.jl @@ -168,21 +168,21 @@ end parse_dampers=false, contact_feet=false, contact_body=false, - limits=false) + joint_limits=Dict()) test_data_system(:walker; parse_springs=false, parse_dampers=false, springs, dampers, contact_feet=false, contact_body=false, - limits=false) + joint_limits=Dict()) test_data_system(:quadruped; parse_springs=false, parse_dampers=false, springs, dampers, contact_feet=false, contact_body=false, - limits=false) + joint_limits=Dict()) for joint_type in joint_types test_data_system(:snake; num_bodies=5, @@ -248,24 +248,21 @@ end parse_springs=false, parse_dampers=false, contact_feet=true, - contact_body=true, - limits=true) + contact_body=true) test_data_system(:walker; parse_springs=false, parse_dampers=false, springs, dampers, contact_feet=true, - contact_body=true, - limits=true) + contact_body=true) test_data_system(:quadruped; parse_springs=false, parse_dampers=false, springs, dampers, contact_feet=true, - contact_body=true, - limits=true) + contact_body=true) for joint_type in joint_types test_data_system(:snake; num_bodies=5, diff --git a/test/energy.jl b/test/energy.jl index d327bf8c5..498563ce3 100644 --- a/test/energy.jl +++ b/test/energy.jl @@ -437,7 +437,7 @@ end springs=springs0, contact_feet=false, contact_body=false, - limits=false) + joint_limits=Dict()) initialize!(mech, :quadruped) storage = simulate!(mech, 5.0; diff --git a/test/joint_limits.jl b/test/joint_limits.jl index e23a0174c..4d92f66ea 100644 --- a/test/joint_limits.jl +++ b/test/joint_limits.jl @@ -4,7 +4,6 @@ gravity=-9.81, springs=0.0, dampers=0.0, - limits=true, joint_limits=Dict([ (:joint, [0.25 * π, 1.0 * π]),]) )