juliarobotics / rigidbodydynamics.jl Goto Github PK

Julia implementation of various rigid body dynamics and kinematics algorithms

License: Other

Julia 100.00%

control dynamics dynamics-models featherstone julia kinematics newtons-laws rigid-body-dynamics robotics robotics-algorithms robotics-control robotics-simulation simulation simulation-modeling

rigidbodydynamics.jl's People

Contributors

Stargazers

Watchers

rigidbodydynamics.jl's Issues

Docstrings for some types in OdeIntegrators module don't show up in documentation

RigidBodyDynamics.OdeIntegrators.RingBufferStorage
RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator
RigidBodyDynamics.OdeIntegrators.ButcherTableau
RigidBodyDynamics.OdeIntegrators.ExpandingStorage

Theory: Documenter issue with parametric types and autodocs.

Allocations related to JointType-specific methods

There are currently lots of allocations in functions like joint_transform, motion_subspace, etc. I believe this is the same issue as https://groups.google.com/forum/#!topic/julia-users/OBs0fmNmjCU. Given this comment and the recent progress on JuliaLang/julia#265, I'm hoping this will be fixed in base one day.

ForwardDiff v0.5.0 hessians not working through RBD functions

I haven't been able to narrow down what the problem is, so this issue is quite verbose.

I'm trying to take ForwardDiff.hessians of RigidBodyDynamics functions: https://gist.github.com/goretkin/e858363daa6a4740934f4fbc48020edc#file-hessians_rbd-jl-L109 and I am getting

MethodError: Cannot `convert` an object of type RigidBodyDynamics.CustomCollections.UnsafeFastDict{RigidBodyDynamics.Graphs.vertex_index,RigidBodyDynamics.RigidBody{Float64},Array{Array{RigidBodyDynamics.Contact.SoftContactStateDeriv{Void,RigidBodyDynamics.Contact.ViscoelasticCoulombStateDeriv{SubArray{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1,Array{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{_,Float64,2},2},1},Tuple{UnitRange{Int64}},true}}},1},1} where _<:ForwardDiff.Tag} to an object of type RigidBodyDynamics.CustomCollections.UnsafeFastDict{RigidBodyDynamics.Graphs.vertex_index,RigidBodyDynamics.RigidBody{Float64},Array{Array{RigidBodyDynamics.Contact.SoftContactStateDeriv{Void,RigidBodyDynamics.Contact.ViscoelasticCoulombStateDeriv{SubArray{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1,Array{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1},Tuple{UnitRange{Int64}},true}}},1},1}}
This may have arisen from a call to the constructor RigidBodyDynamics.CustomCollections.UnsafeFastDict{RigidBodyDynamics.Graphs.vertex_index,RigidBodyDynamics.RigidBody{Float64},Array{Array{RigidBodyDynamics.Contact.SoftContactStateDeriv{Void,RigidBodyDynamics.Contact.ViscoelasticCoulombStateDeriv{SubArray{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1,Array{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1},Tuple{UnitRange{Int64}},true}}},1},1}}(...),
since type constructors fall back to convert methods.
Stacktrace:
 [1] RigidBodyDynamics.DynamicsResult{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},M} where M(::RigidBodyDynamics.Mechanism{Float64}) at /Users/goretkin/playgrounds/jump/packages/v0.6/RigidBodyDynamics/src/dynamics_result.jl:73
 [2] evaluate_friction_kinetic_energy(::RigidBodyDynamics.MechanismState{Float64,Float64,Float64,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}, ::Float64, ::Float64, ::Array{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1}) at /Users/goretkin/projects/julia_lqr/min_jump.jl:63
 [3] vector_mode_gradient at /Users/goretkin/playgrounds/jump/packages/v0.6/ForwardDiff/src/gradient.jl:94 [inlined]
 [4] gradient(::##2#3, ::Array{ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},1}, ::ForwardDiff.GradientConfig{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2,Array{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1}}) at /Users/goretkin/playgrounds/jump/packages/v0.6/ForwardDiff/src/gradient.jl:19
 [5] vector_mode_jacobian(::ForwardDiff.##43#44{##2#3,ForwardDiff.HessianConfig{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},Float64,2,Array{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1},0xb046287d533b082d,Array{ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},1}}}, ::Array{Float64,1}, ::ForwardDiff.JacobianConfig{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2,Array{ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},1}}) at /Users/goretkin/playgrounds/jump/packages/v0.6/ForwardDiff/src/jacobian.jl:132
 [6] jacobian(::ForwardDiff.##43#44{##2#3,ForwardDiff.HessianConfig{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},Float64,2,Array{ForwardDiff.Dual{ForwardDiff.Tag{##2#3,0x096989b645fcecd8},ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},2},1},0xb046287d533b082d,Array{ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},1}}}, ::Array{Float64,1}, ::ForwardDiff.JacobianConfig{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2,Array{ForwardDiff.Dual{ForwardDiff.Tag{Void,0xb046287d533b082d},Float64,2},1}}) at /Users/goretkin/playgrounds/jump/packages/v0.6/ForwardDiff/src/jacobian.jl:21
 [7] hessian(::##2#3, ::Array{Float64,1}) at /Users/goretkin/playgrounds/jump/packages/v0.6/ForwardDiff/src/hessian.jl:19
 [8] include_from_node1(::String) at ./loading.jl:569
 [9] include(::String) at ./sysimg.jl:14

It's hard to read all those nested types, here they are indented:
from type:

RigidBodyDynamics.CustomCollections.UnsafeFastDict{
  RigidBodyDynamics.Graphs.vertex_index,
  RigidBodyDynamics.RigidBody{
    Float64},
  Array{
    Array{
      RigidBodyDynamics.Contact.SoftContactStateDeriv{
        Void,
        RigidBodyDynamics.Contact.ViscoelasticCoulombStateDeriv{
          SubArray{
            ForwardDiff.Dual{
              ForwardDiff.Tag{
                ##2#3,
                0x096989b645fcecd8},
              ForwardDiff.Dual{
                ForwardDiff.Tag{
                  Void,
                  0xb046287d533b082d},
                Float64,
                2},
              2},
            1,
            Array{
              ForwardDiff.Dual{
                ForwardDiff.Tag{
                  ##2#3,
                  0x096989b645fcecd8},
                ForwardDiff.Dual{
                  _,          # this line differs
                  Float64,
                  2},
                2},
              1},
            Tuple{
              UnitRange{
                Int64}},
            true}}},
      1},
    1}
where _<:ForwardDiff.Tag} # this line differs

to type:

RigidBodyDynamics.CustomCollections.UnsafeFastDict{
  RigidBodyDynamics.Graphs.vertex_index,
  RigidBodyDynamics.RigidBody{
    Float64},
  Array{
    Array{
      RigidBodyDynamics.Contact.SoftContactStateDeriv{
        Void,
        RigidBodyDynamics.Contact.ViscoelasticCoulombStateDeriv{
          SubArray{
            ForwardDiff.Dual{
              ForwardDiff.Tag{
                ##2#3,
                0x096989b645fcecd8},
              ForwardDiff.Dual{
                ForwardDiff.Tag{
                  Void,
                  0xb046287d533b082d},
                Float64,
                2},
              2},
            1,
            Array{
              ForwardDiff.Dual{
                ForwardDiff.Tag{
                  ##2#3,
                  0x096989b645fcecd8},
                ForwardDiff.Dual{
                  ForwardDiff.Tag{      # there is a concrete tag
                    Void,
                    0xb046287d533b082d},
                  Float64,
                  2},
                2},
              1},
            Tuple{
              UnitRange{
                Int64}},
            true}}},
      1},
1}}

This doesn't appear to be an issue with the previous version of ForwardDiff, v0.4.2. It may very well be an issue with ForwardDiff itself, but I thought I'd bring it up here first since I wasn't able to distill the example.

Illegal instruction on Julia nightly

Unreachable reached at 0x14473426f

signal (4): Illegal instruction: 4
while loading /Users/twan/code/julia/RigidBodyDynamics/v0.7/RigidBodyDynamics/test/test_double_pendulum.jl, in expression starting on line 1
macro expansion at /Users/twan/code/julia/RigidBodyDynamics/v0.7/RigidBodyDynamics/test/test_double_pendulum.jl:38 [inlined]
macro expansion at ./test.jl:856 [inlined]
anonymous at ./<missing> (unknown line)
jl_call_fptr_internal at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/./julia_internal.h:353 [inlined]
jl_call_method_internal at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/./julia_internal.h:372 [inlined]
jl_toplevel_eval_flex at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/toplevel.c:589
jl_parse_eval_all at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/ast.c:907
jl_load at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/toplevel.c:616 [inlined]
jl_load_ at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/toplevel.c:623
include_from_node1 at ./loading.jl:551
unknown function (ip: 0x11fa06922)
include at ./sysimg.jl:14
jlcall_include_1034 at /Applications/Julia-0.7.app/Contents/Resources/julia/lib/julia/sys.dylib (unknown line)
do_call at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/interpreter.c:75
eval at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/interpreter.c:242
jl_interpret_toplevel_expr at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/interpreter.c:34
jl_toplevel_eval_flex at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/toplevel.c:577
jl_parse_eval_all at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/ast.c:907
jl_load at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/toplevel.c:616 [inlined]
jl_load_ at /Users/osx/buildbot/slave/package_osx10_9-x64/build/src/toplevel.c:623
include_from_node1 at ./loading.jl:551
jlcall_include_from_node1_18673 at /Applications/Julia-0.7.app/Contents/Resources/julia/lib/julia/sys.dylib (unknown line)
include at ./sysimg.jl:14
jlcall_include_1034 at /Applications/Julia-0.7.app/Contents/Resources/julia/lib/julia/sys.dylib (unknown line)
process_options at ./client.jl:307
_start at ./client.jl:373
jlcall__start_18883 at /Applications/Julia-0.7.app/Contents/Resources/julia/lib/julia/sys.dylib (unknown line)
true_main at /Applications/Julia-0.7.app/Contents/Resources/julia/bin/julia (unknown line)
main at /Applications/Julia-0.7.app/Contents/Resources/julia/bin/julia (unknown line)
Allocations: 27221398 (Pool: 27215765; Big: 5633); GC: 40

Quick start guide?

I'm potentially interested in a Julia solution for Deep Reinforcement Learning with robotic simulation. Right now I'm using the Box2D environment for continuous control problems using my packages Reinforce and my wrapper around OpenAIGym.

Your package(s) sound like a possible alternative/solution, especially when considering 3D simulations, but I'm having a hard time wrapping my head around the easiest way to make a simple reinforcement learning enviroment using RigidBodyDynamics. Does this sound like a good use for your software? Can you help me get started (or better yet, do you have interest collaborating on building environments?)

Generalizing the diffeqs

Hey,
I was wondering what kinds of diffeq methods you guys would like to see, as well as what a good general API would be. I see that you use Lie group integrators here, and I am not too familiar with them, so I was wondering if you could help JuliaDiffEq design an interface for Lie Group integrators that methods can be built off of. Specifically:

What would the data of a LieGroupProblem need to have? It would have the ODE f of course, but how would the Lie group be specified? Is there a good general way to do this?
How do these compare to other symplectic methods?
Do you have an internal version of expmv you're using for this?

Coarser cache checks in MechanismState

Currently, there is a CacheElement for every joint transform, every joint twist, etc. This means that every joint transform access goes through an if statement, which incurs a computation time penalty (branching). Also, the current last-minute, lazy computation style is probably bad in terms of temporal cache locality. Invalidating all the cache elements currently requires a lot of iteration as well.

It would likely be better to e.g. compute all joint transforms and transforms to world in one go if a single transform is required, because it is likely that all the other ones are needed as well. This is especially true when computing the mass matrix or inverse dynamics.

Two possible options to fix this:

store e.g. CacheElement{Vector{Transform3D}} in MechanismState and change @cache_element_get to do its update in place.
keep the current setup of one CacheElement per joint transform, but manually do all the updates in one go in e.g. mass_matrix, after which the cache elements are accessed in an unsafe fashion, bypassing the if branch.

Inner constructor docstrings don't make it into documentation

Fix tolerance problem in local / global coordinates test.

https://github.com/tkoolen/RigidBodyDynamics.jl/compare/switch-to-rotations?expand=1#diff-723fbb811a63b5a885ae4a3f95a0612bR351

Introduced in #134.

Adding a new body-fixed frame?

I'm trying to refactor RigidBodyTreeInspector by attaching each visualized geometry to a specific frame in the manipulator. That means creating a new body-fixed frame for each geometry. Should the following work?

julia> using RigidBodyDynamics

julia> mechanism = rand_chain_mechanism(Float64, Revolute{Float64})
Vertex: world (root)
  Vertex: body1, Edge: joint1

julia> body = mechanism.toposortedTree[2].vertexData
RigidBody: "body1"

julia> frame = CartesianFrame3D("new frame")
CartesianFrame3D: "new frame"

julia> tform = Transform3D(body.frame, frame, StaticArrays.SVector(1., 0, 0))
Transform3D from "body1" to "new frame":
rotation: 0.0 rad about [1.0,0.0,0.0], translation: [1.0,0.0,0.0]


julia> add_body_fixed_frame!(mechanism, tform)
ERROR: KeyError: key CartesianFrame3D: "new frame" not found
 in getindex at ./dict.jl:688 [inlined]
 in add_body_fixed_frame!(::RigidBodyDynamics.Mechanism{Float64}, ::RigidBodyDynamics.Transform3D{Float64}) at /Users/rdeits/locomotion/explorations/point-cloud-signed-distance/packages/v0.5/RigidBodyDynamics/src/mechanism.jl:89

Don't exploit sparsity in Transform3D, SpatialInertia?

Especially on newer CPU architectures, it may be favorable not to exploit sparsity in e.g. multiplication of homogeneous transforms.

AVX2-capable machine:

Julia Version 0.6.0-pre.beta.295
Commit dc907c7 (2017-04-24 04:37 UTC)
Platform Info:
  OS: Linux (x86_64-pc-linux-gnu)
  CPU: Intel(R) Core(TM) i7-6950X CPU @ 3.00GHz
  WORD_SIZE: 64
  BLAS: libopenblas (USE64BITINT DYNAMIC_ARCH NO_AFFINITY Haswell)
  LAPACK: libopenblas64_
  LIBM: libopenlibm
  LLVM: libLLVM-3.9.1 (ORCJIT, broadwell)

Older, non-AVX2-capable machine:

Julia Version 0.6.0-pre.beta.295
Commit dc907c760f (2017-04-24 04:37 UTC)
Platform Info:
  OS: macOS (x86_64-apple-darwin13.4.0)
  CPU: Intel(R) Core(TM) i7-3820QM CPU @ 2.70GHz
  WORD_SIZE: 64
  BLAS: libopenblas (USE64BITINT DYNAMIC_ARCH NO_AFFINITY Sandybridge)
  LAPACK: libopenblas64_
  LIBM: libopenlibm
  LLVM: libLLVM-3.9.1 (ORCJIT, ivybridge)

In each case, I rebuilt the system image for the native architecture.

Exploiting sparsity

@benchmark (arot * brot, atrans + arot * btrans) setup = begin
    arot = rand(SMatrix{3, 3})
    brot = rand(SMatrix{3, 3})
    atrans = rand(SVector{3})
    btrans = rand(SVector{3})
end

AVX2:

BenchmarkTools.Trial: 
  memory estimate:  0 bytes
  allocs estimate:  0
  --------------
  minimum time:     12.423 ns (0.00% GC)
  median time:      12.496 ns (0.00% GC)
  mean time:        12.906 ns (0.00% GC)
  maximum time:     32.182 ns (0.00% GC)
  --------------
  samples:          10000
  evals/sample:     999

Non-AVX2:

BenchmarkTools.Trial: 
  memory estimate:  0 bytes
  allocs estimate:  0
  --------------
  minimum time:     10.598 ns (0.00% GC)
  median time:      11.208 ns (0.00% GC)
  mean time:        11.527 ns (0.00% GC)
  maximum time:     91.898 ns (0.00% GC)
  --------------
  samples:          10000
  evals/sample:     999
  time tolerance:   5.00%
  memory tolerance: 1.00%

Not exploiting sparsity

@benchmark a * b setup = (a = rand(SMatrix{4, 4}); b = rand(SMatrix{4, 4}))

AVX2:

BenchmarkTools.Trial: 
  memory estimate:  0 bytes
  allocs estimate:  0
  --------------
  minimum time:     5.331 ns (0.00% GC)
  median time:      5.344 ns (0.00% GC)
  mean time:        5.565 ns (0.00% GC)
  maximum time:     26.861 ns (0.00% GC)
  --------------
  samples:          10000
  evals/sample:     1000

Non-AVX2:

BenchmarkTools.Trial: 
  memory estimate:  0 bytes
  allocs estimate:  0
  --------------
  minimum time:     7.679 ns (0.00% GC)
  median time:      8.138 ns (0.00% GC)
  mean time:        8.520 ns (0.00% GC)
  maximum time:     54.870 ns (0.00% GC)
  --------------
  samples:          10000
  evals/sample:     999
  time tolerance:   5.00%
  memory tolerance: 1.00%

Contact dynamics

Store collision geometry in Mechanism (current status: contact points stored in RigidBody, world-fixed environment in the form of halfspaces stored in Mechanism)
Decide how to do distance queries (GJK implementation from GeometryTypes?).
Penalty-based contact dynamics
LCP-style contact dynamics
Parsing collision elements from URDF.

Allocations related to caching homogeneous transforms

UpdateTransformToRoot callable currently doesn't fully specify field types.
TransformCache currently doesn't fully specify field types.
...

Find solution for motion_subspace type instability without allocating Arrays

Currently, the _motion_subspace functions associated with the different JointTypes return different GeometricJacobian types (with different underlying data types for the angular and linear fields), because they have different sizes. A possible solution is to always return a GeometricJacobian{A} with A a view of a StaticArrays.SMatrix{3, 6}.

Replace RTTI-style dispatch on joint types

Currently, RTTI-style dispatch is needed to call joint-type-dependent methods (e.g. joint_transform) in a type-stable way. This has two main drawbacks:

it makes it impossible for users to define their own joint types (currently, QuaternionFloating, Revolute, Prismatic, Fixed are hardcoded as the only supported types)
there is some overhead associated with the if-tree in the @rtti_dispatch macro.

The current plan to fix this is as follows:

Instead of parameterizing Joint on the scalar type T, parameterize it on JointType{T}
instead of storing Joint{T}s in Mechanism, simply store Joints
in the MechanismState constructor, sort the joints by their JointType parameter, and store them in a Tuple of Vectors. Parameterize MechanismState on this tuple type. For example, if the Mechanism only contains Revolute{Float64} and Prismatic{Float64} joints, store them in a Tuple{Vector{Joint{Revolute{Float64}}}, Vector{Joint{Prismatic{Float64}}}} in MechanismState.
to update e.g. the joint transforms in MechanismState, iterate over all vectors in the joint tuple and all elements in each of the vectors, and call joint_transform with the concrete Joint subtypes, storing the results in a Vector{<:Transform3D}.

Enforce that joint names and body names are unique

Needed because of rdeits/RigidBodyTreeInspector.jl#29.

Consider moving gravitational acceleration outside of Mechanism

... and instead making it be an argument of inverse_dynamics! and related functions.

Update UnsafeVectorView (doesn't work for non-isbits types)

Based on @rdeits's changes in https://github.com/rdeits/NNLS.jl/blob/0a9bf56774595b5735bc738723bd3cb94138c5bd/src/NNLS.jl#L200.

Benchmarks have gotten slower

I can no longer reproduce the benchmark results in the documentation, even with release 0.0.4, which makes it seem likely that it's at least partly a third party issue.

Crazy idea: static char arrays instead of global frame_names

The global frame_names map has a somewhat scary problem in that every new coordinate frame causes that list to grow, and old entries can never be deleted. So far this hasn't actually cause any measurable issues for me, but it's still concerning.

What if, instead, we used a fixed-size static array of Chars instead of the Int64 reference? That might look something like:

immutable CartesianFrame3D
    name::SVector{64, Char}
    function CartesianFrame3D(name::String)
        length(name) > 64 && warn("Frame name longer than 64 characters was truncated")
        new(SVector{64, Char}(Base.flatten((name[1:min(length(name), 64)], ('\x0' for i in 1:(64 - length(name)))))...))
    end
end

show(io::IO, frame::CartesianFrame3D) = print(io, "CartesianFrame3D: $(join(frame.name[frame.name .!= '\x0'], ""))")

This has the obvious problem that it makes the stack-allocated size of a frame much, much larger. That alone might be enough to make this a very bad idea. However, it does result in an isbits type that doesn't require a global reference.

Make joints(m::Mechanism) and bodies(m::Mechanism) not allocate new arrays

Erroneous contact resets in `contact_dynamics!` when there is more than one element in `ContactEnvironment`

https://github.com/tkoolen/RigidBodyDynamics.jl/blob/afb2236dd916ab0dcf8e86f93ffcb5db96414511/test/test_simulate.jl#L89 fails if another element is added to the contact environment. This is because there should be contact state for every combination of a contact point and a contact environment element; currently there is only state associated with each contact point.

Documentation build error

Due to JuliaDocs/DocStringExtensions.jl#32.

Type instabilities in findfirst functions for Tree

[v0.6-rc2] Error running the symbolic double pendulum example

I get this error from both release and master branch while running the example at: http://nbviewer.jupyter.org/github/tkoolen/RigidBodyDynamics.jl/blob/master/notebooks/Symbolic%20double%20pendulum.ipynb

MethodError: convert(::Type{RigidBodyDynamics.SpatialInertia{SymPy.Sym}}, ::RigidBodyDynamics.SpatialInertia{SymPy.Sym}) is ambiguous. Candidates:
  convert(::Type{RigidBodyDynamics.SpatialInertia{T}}, inertia::RigidBodyDynamics.SpatialInertia) where T<:Number in RigidBodyDynamics at /home/smldis/.julia/v0.6/RigidBodyDynamics/src/spatial.jl:185
  convert(::Type{RigidBodyDynamics.SpatialInertia{T}}, inertia::RigidBodyDynamics.SpatialInertia{T}) where T in RigidBodyDynamics at /home/smldis/.julia/v0.6/RigidBodyDynamics/src/spatial.jl:182
Possible fix, define
  convert(::Type{RigidBodyDynamics.SpatialInertia{T<:Number}}, ::RigidBodyDynamics.SpatialInertia{T<:Number})

Stacktrace:
 [1] Type at /home/smldis/.julia/v0.6/RigidBodyDynamics/src/rigid_body.jl:24 [inlined]
 [2] RigidBodyDynamics.RigidBody(::RigidBodyDynamics.SpatialInertia{SymPy.Sym}) at /home/smldis/.julia/v0.6/RigidBodyDynamics/src/rigid_body.jl:31
 [3] execute_request(::ZMQ.Socket, ::IJulia.Msg) at /home/smldis/.julia/v0.6/IJulia/src/execute_request.jl:156
 [4] eventloop(::ZMQ.Socket) at /home/smldis/.julia/v0.6/IJulia/src/eventloop.jl:8
 [5] (::IJulia.##9#12)() at ./task.jl:335

For now I hacked it away by deleting the line 182 of spatial.jl.

Do you get the same error?

Modifying the jointType field can cause the bounds to be incorrect

The ValkyrieRobot repo constructs Val by doing

pelvis_to_world = joint_to_parent(pelvis, mechanism)
pelvis_to_world.jointType = QuaternionFloating{Float64}()

but this is probably not a good idea anymore, since it means that the Joint's position/velocity/effort bounds fields will not be updated to reflect the new number of degrees of freedom. We can fix this by providing a setter that resets the bounds of the joint when its type changes.

Implement closed-loop mechanisms

Add constraint force subspace function for each joint type.
Store loop joints in Mechanism
Modify attach! to handle loop joints.
Enforce loop joint constraints in dynamics.
Initial resolution of loop joint constraints using a nonlinear program.
Constraint projection
~~Consider using a Graphs.jl Graph as the main data structure used in Mechanism.~~ decided to roll my own.
Modify reattach! etc. to handle loop joints.
constraint stabilization

Add joint limit functionality

Store joint limits in Mechanism or Joint.
Take joint limits into account in dynamics.
URDF parsing.

Add functionality to renormalize a `MechanismState`

So that q lies on the configuration manifold (currently; just entails renormalizing quaternions for QuaternionFloating and QuaternionSpherical joints).

Segfault while running tests

Seems to be because of SimpleTraits.jl update to 0.2.0.

See mauro3/SimpleTraits.jl#21.

Error when constructing MechanismState for an attached mechanism

When constructing a new MechanismState after attaching two mechanisms, I get one of two possible errors:

julia> MechanismState(Float64, mechanism)
ERROR: KeyError: key CartesianFrame3D: "world" not found
 in getindex at ./dict.jl:688 [inlined]
 in RigidBodyDynamics.MechanismState{X<:Real,M<:Real,C<:Real}(::Type{Float64}, ::RigidBodyDynamics.Mechanism{Float64}) at /Users/rdeits/locomotion/explorations/point-cloud-signed-distance/packages/v0.5/RigidBodyDynamics/src/mechanism_state.jl:196

julia> MechanismState(Float64, mechanism)
ERROR:
 in getindex at ./dict.jl:688 [inlined]
 in RigidBodyDynamics.MechanismState{X<:Real,M<:Real,C<:Real}(::Type{Float64}, ::RigidBodyDynamics.Mechanism{Float64}) at /Users/rdeits/locomotion/explorations/point-cloud-signed-distance/packages/v0.5/RigidBodyDynamics/src/mechanism_state.jl:196
SYSTEM: show(lasterr) caused an error

Which error I get is unpredictable. A given mechanism instance will always throw the same error every time I try to construct MechanismState, but running the exact same code (with the RNG seed fixed) to generate a new mechanism may result in one which throws the other error.

The code I'm using to generate the attached mechanism is:

srand(0)
mechanism = rand_tree_mechanism(Float64, [Revolute{Float64} for i = 1 : 2]...)
mechanism2 = rand_tree_mechanism(Float64, [Revolute{Float64} for i = 1 : 2]...)
connection = Joint("connection", rand(Revolute{Float64}))
parentBody = collect(bodies(mechanism))[2]
attach!(mechanism, parentBody, connection, rand(Transform3D{Float64}, connection.frameBefore, parentBody.frame), mechanism2)

Add modification count to Mechanism

Store it in MechanismState upon construction, check that it matches somewhere, probably update_transforms!.

DimensionMismatch("Lengths of input collections do not match.")

using RigidBodyDynamics

urdf_text = """
<?xml version="1.0" ?>
<robot name="prism2">

  <link name="base_link">
    <inertial>
      <mass value="1.0"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>

  <link name="middle_link">
    <inertial>
      <mass value="1.0"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>

  <link name="tip_link">
    <inertial>
      <mass value="1.0"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>

  <joint name="joint1" type="prismatic">
    <axis xyz="1 0 0"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
    <parent link="base_link"/>
    <child link="middle_link"/>
  </joint>

  <joint name="joint2" type="prismatic">
    <axis xyz="1 0 0"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
    <parent link="middle_link"/>
    <child link="tip_link"/>
  </joint>
</robot>
"""

open("/tmp/urdf_unique", "w") do f
    write(f, urdf_text);
end

mechanism = parse_urdf(Float64, "/tmp/urdf_unique")
x = MechanismState(mechanism)
rand!(x)
times, qs, vs = simulate(x, 0.1; Δt = 1e-2)


"""
DimensionMismatch("Lengths of input collections do not match.")

Stacktrace:
 [1] lengths_match_fail() at /Users/goretkin/playgrounds/rbd_diff/packages/v0.6/TypeSortedCollections/src/TypeSortedCollections.jl:119
 [2] macro expansion at /Users/goretkin/playgrounds/rbd_diff/packages/v0.6/RigidBodyDynamics/src/custom_collections.jl:26 [inlined]
 [3] foreach_with_extra_args at /Users/goretkin/playgrounds/rbd_diff/packages/v0.6/RigidBodyDynamics/src/custom_collections.jl:8 [inlined]
 [4] global_coordinates!(::RigidBodyDynamics.MechanismState{Float64,Float64,Float64,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Fixed{Float64}},1},Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Prismatic{Float64}},1}},2}}, ::Array{Float64,1}, ::Array{Float64,1}) at /Users/goretkin/playgrounds/rbd_diff/packages/v0.6/RigidBodyDynamics/src/mechanism_state.jl:842
 [5] step(::RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator{4,Float64,RigidBodyDynamics.##147#148{RigidBodyDynamics.DynamicsResult{Float64,Float64}},RigidBodyDynamics.OdeIntegrators.ExpandingStorage{Float64},16}, ::Float64, ::RigidBodyDynamics.MechanismState{Float64,Float64,Float64,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Fixed{Float64}},1},Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Prismatic{Float64}},1}},2}}, ::Float64) at /Users/goretkin/playgrounds/rbd_diff/packages/v0.6/RigidBodyDynamics/src/ode_integrators.jl:269
 """

By instrumenting TypeSortedCollections.lengths_match the failing arguments are:

a1 = 3
a2 = SubArray{Float64,1,Array{Float64,1},Tuple{UnitRange{Int64}},true}[Float64[], [2.30232], [-0.0928717]]
length(a1) = 1
length(a2) = 3

This URDF was not giving me trouble when using a master from two months ago (sorry, I lost the specific tag. I can do a bisect later today).

Generalize spatial types: allow any backing array type

Currently, Point3D, FreeVector3D, GeometricJacobian, MomentumMatrix, and WrenchMatrix are parameterized on their underlying array types.

On the other hand, Transform3D, SpatialInertia, Twist, SpatialAcceleration, Wrench, and Momentum are parameterized on the scalar type, and have StaticArray backing arrays hardcoded.

These types should also be parameterized on the backing array type, because:

it enables creating mutable versions of these types
it is required for ReverseDiff support, because of JuliaDiff/ReverseDiff.jl#64 (comment)
it would be more consistent with the other types

Issue with conditional import of Base.Iterators in runtests.jl

We saw this on @Symplectomorphism 's machine. It looks like the conditional from RigidBodyDynamics.jl should be copied over into runtests.jl

Modularize things more

I kind of like the way Documenter has been modularized; consider doing the same.

New release checklist

Mostly because of #332.

run tests of previous version on master to catch missing deprecation warnings
check benchmarks
test notebooks (esp. visualization stuff that is not tested on Travis)
news
release notes

Consider switching from Quaternions.jl to Rotations.jl or CoordinateTransformations.jl

See 866fca5.

Quaternions.jl is barely maintained, and not having to implement conversions to other rotation parameterizations in RigidBodyDynamics.jl would be nice.

Cons of switching to Rotations.jl: currently, Rotations.jl's quaternion type is really a unit quaternion only, and always normalizes upon construction. There are a couple of places where it would be nice to have non-normalized quaternions (mostly for quaternion derivatives). Also, it wouldn't be possible to get the dynamics in trigonometric polynomial form if the quaternion is normalized upon construction.

Proper integration of quaternion floating joint state

Currently, the unit norm constraint is not enforced in the dynamics. Consider either adding constraint stabilization to the dynamics, or supplying a specialized numerical integrator that is quaternion-aware (see e.g. http://ieeexplore.ieee.org/document/1512344/).

Keyword args for attach!

The attach! methods have a lot of positional arguments. Consider turning the poses into keyword arguments.

Error tagging new release

The tag name "0.0.5" is not of the appropriate SemVer form (vX.Y.Z).
cc: @tkoolen

`show()` only shows the root link of a mechanism

It used to show the whole tree. Was that an intentional change?

julia> mechanism = rand_chain_mechanism(Float64, [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1:5]]...)
Vertex: world (root)

julia> tree(mechanism)
Vertex: world (root)
  Vertex: body1, Edge: joint1
    Vertex: body2, Edge: joint2
      Vertex: body3, Edge: joint3
        Vertex: body4, Edge: joint4
          Vertex: body5, Edge: joint5
            Vertex: body6, Edge: joint6

Duplicate body names when attaching two mechanisms

I have been (implicitly) assuming in RigidBodyTreeInspector that all the links in a robot will have unique names. That assumption comes from the drake-visualizer interface, which uses the link names as the only way of identifying links within a robot. However, when I attach!() one mechanism onto another, the combined mechanism will have duplicate link names.

How do you want to handle this situation? Should RigidBodyDynamics enforce uniqueness of the link names? Or should I just make RigidBodyTreeInspector more robust to this case? Really, I should do the latter anyway, but it's still good to decide if uniqueness of link names is a safe assumption.

Feature request: Planar joint type

`num_positions(::Mechanism)` is type-unstable

julia> using RigidBodyDynamics

julia> m = parse_urdf(Float64, "test/urdf/Acrobot.urdf")
Spanning tree:
Vertex: world (root)
  Vertex: base_link, Edge: base_link_to_world
    Vertex: upper_link, Edge: shoulder
      Vertex: lower_link, Edge: elbow
No non-tree joints.

julia> @code_warntype num_positions(m)
Variables:
  #self#::RigidBodyDynamics.#num_positions
  mechanism::RigidBodyDynamics.Mechanism{Float64}

Body:
  begin 
      SSAValue(0) = (Core.getfield)((Core.getfield)(mechanism::RigidBodyDynamics.Mechanism{Float64}, :tree)::RigidBodyDynamics.Graphs.SpanningTree{RigidBodyDynamics.RigidBody{Float64},RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.JointType{Float64}}}, :edges)::Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.JointType{Float64}},1}
      return $(Expr(:invoke, MethodInstance for mapfoldl_impl(::RigidBodyDynamics.#num_positions, ::Base.#+, ::Int64, ::Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.JointType{Float64}},1}, ::Int64), :(Base.mapfoldl_impl), :(RigidBodyDynamics.num_positions), :(RigidBodyDynamics.+), 0, SSAValue(0), 1))
  end::Any

I'll see if I can resolve this.

New release checklist

#280
StaticArrays tag (JuliaArrays/StaticArrays.jl#276)
update benchmark results
add more stuff to README.md
run tests of previous version on master to catch missing deprecation warnings
consider adding error for geometric_jacobian! change (compact to full)
news
release notes

juliarobotics / rigidbodydynamics.jl Goto Github PK

rigidbodydynamics.jl's People

Contributors

Stargazers

Watchers

Forkers

rigidbodydynamics.jl's Issues

Exploiting sparsity

Not exploiting sparsity

Recommend Projects

Recommend Topics

Recommend Org