diff --git a/DojoEnvironments/src/panda/methods/initialize.jl b/DojoEnvironments/src/panda/methods/initialize.jl index c5591c92c..db5ec46b3 100644 --- a/DojoEnvironments/src/panda/methods/initialize.jl +++ b/DojoEnvironments/src/panda/methods/initialize.jl @@ -156,13 +156,13 @@ function get_panda(; return mech end -# function initialize_panda!(mechanism::Mechanism{T}; -# joint_angles=[[0,-0.8,0,1.6,0,-2.4,0]; zeros(input_dimension(mechanism)-7)], -# joint_velocities=zeros(input_dimension(mechanism))) where T - -# nu = input_dimension(mechanism) -# zero_velocity!(mechanism) -# y = vcat([[joint_angles[i], joint_velocities[i]] for i=1:nu]...) -# set_minimal_state!(mechanism, y) -# return nothing -# end +function initialize_panda!(mechanism::Mechanism{T}; + joint_angles=[[0,-0.8,0,1.6,0,-2.4,0]; zeros(input_dimension(mechanism)-7)], + joint_velocities=zeros(input_dimension(mechanism))) where T + + nu = input_dimension(mechanism) + zero_velocity!(mechanism) + y = vcat([[joint_angles[i], joint_velocities[i]] for i=1:nu]...) + set_minimal_state!(mechanism, y) + return nothing +end