rigid

package
v0.0.0-...-ba3cd27 Latest Latest
Warning

This package is not in the latest version of its module.

 Go to latest Published: Feb 19, 2022 License: MIT Imports: 4 Imported by: 0

Documentation

Index

Constants

This section is empty.

Variables

This section is empty.

Functions

func Cross 

func Cross(a, b mat.Matrix) *mat.Dense

func MakeJointMap 

func MakeJointMap(mov []*Joint) map[string][]float64

func MatPrint 

func MatPrint(m mat.Matrix)

func OdeSolver 

func OdeSolver(fn Ode, t0 float64, x0 *mat.Dense, step, tn float64) *mat.Dense

func RPY 

func RPY(w, p, r float64) *mat.Dense

func ReadTorques 

func ReadTorques(mov []*Joint) *mat.Dense

Return joint torques in form of vector

func Rx 

func Rx(q float64) *mat.Dense

func Ry 

func Ry(q float64) *mat.Dense

func Rz 

func Rz(q float64) *mat.Dense

func Txyz 

func Txyz(x, y, z float64) *mat.Dense

Types

type Ik6_Geometry 

type Ik6_Geometry struct {
	A    [3]float64
	B    float64
	C    [5]float64
	Dq   [6]float64 // "deflections" in joints
	Name [6]string  // joint names
	Q    *mat.Dense // matrix of solutions, each solution in separate column
	R    *mat.Dense // correct rotation
}

func (*Ik6_Geometry) Closest 

func (par *Ik6_Geometry) Closest(prev []float64) int

Find closest solution for the previous state return -1 if solution not found

func (*Ik6_Geometry) ClosestTo 

func (par *Ik6_Geometry) ClosestTo(qs map[string][]float64) int

Find closest solution based on joint map

func (*Ik6_Geometry) IkFull 

func (par *Ik6_Geometry) IkFull(rot, pos *mat.Dense)

Inverse kinematics Return matrix with 8 solutions Based on Mathias Brandstotter, "An analytical solution of the inverse kinematics problem of industrial serial manipulators ..."

func (*Ik6_Geometry) SetTo 

func (par *Ik6_Geometry) SetTo(qs map[string][]float64, col int)

Save solution into joint map

type Inertial 

type Inertial struct {
	I  *mat.Dense // inertia matrix
	Rc *mat.Dense // mass center
	M  float64    // link mass
}

Inertial parameters of link

type Joint 

type Joint struct {
	// source
	Src *urdf.Joint
	// tree
	Parent *Link
	Child  *Link
	// type
	Type JointType
	// Parameters
	Angle float64
	Vel   float64
	Acc   float64
	// Transformations
	Trans Transform // constant transformation
	Local Transform // rotation from current to next
	Limit [2]float64
	//Rij           *mat.Dense    // from current to next joint
	Axis *mat.Dense // joint axis
	// dynamics
	Tau float64
}

func (*Joint) GetCopy 

func (src *Joint) GetCopy() *Joint

func (*Joint) InRange 

func (jnt *Joint) InRange(q float64) bool

func (*Joint) UpdateLocal 

func (jnt *Joint) UpdateLocal(q float64)

Update current rotation transform

type JointType 

type JointType int

Joint classification 

type Link struct {
	// source
	Src *urdf.Link
	// tree
	Joints []*Joint
	Parent *Joint
	// current state
	State Transform
	// inertial parameters
	Dyn Inertial
}

func BodyTree 

func BodyTree(model *urdf.Model) *Link

Make tree of rigid body elements

func (*Link) Find 

func (v *Link) Find(name string) *Link

Find link with the given name

func (*Link) FindIk6Param 

func (base *Link) FindIk6Param(ee *Link) *Ik6_Geometry

Find parameters if the robot IK can be calculated via analytical solution for 6 joints

func (*Link) GetCopy 

func (src *Link) GetCopy() *Link

func (*Link) Jacobian 

func (ee *Link) Jacobian(mov []*Joint) *mat.Dense

Calculate Jacobian matrix

func (*Link) Predecessors 

func (v *Link) Predecessors() []*Joint

collect movable joints

func (*Link) UpdateDyn 

func (base *Link) UpdateDyn(g float64)

Find dymanical state using RNEA algorithm

func (*Link) UpdateState 

func (v *Link) UpdateState(qs map[string][]float64)

Update chain parameters for the given joint states

type Ode 

type Ode func(float64, *mat.Dense) *mat.Dense

type Path 

type Path struct {
	Joints [][]float64
	S      []float64 // when S != nil use it for steps
}

func (Path) GetLinear 

func (p Path) GetLinear(s float64, res []float64) bool

Linear interpolation 0 <= s <= 1

func (Path) GetSquare 

func (p Path) GetSquare(s float64, res []Polynomial) (bool, float64, float64)

square interpolation 0 <= s <= 1 return result and interval for s

func (Path) Ind 

func (p Path) Ind(s float64) int

Find closest index for a given s

type Polynomial 

type Polynomial []float64

Use arrays as polynomial coefficients

func (Polynomial) Val 

func (p Polynomial) Val(x float64) float64

Find value

func (Polynomial) Val1d 

func (p Polynomial) Val1d(x float64) float64

Evaluate d/dx value

func (Polynomial) Val2d 

func (p Polynomial) Val2d(x float64) float64

Evaluate d2/dx2 value

type Profile 

type Profile struct {
	State [3]Polynomial // [lift off, travel, set down]
	Time  []float64     // time markers
	T     float64       // total period (minimal)
	D     float64       // total length
}

Speed profile representation

func Trapez 

func Trapez(d, vmax, amax float64) *Profile

Find trapez profile for distance d with limits vmax, amax

func (*Profile) At 

func (p *Profile) At(k, period float64, res []float64) float64

Calculate state at relative time k, return absolute time

func (*Profile) AtScale 

func (p *Profile) AtScale(k, factor float64, res []float64) float64

Calculate state for relative time and expanded period, return absolute time

type Transform 

type Transform struct {
	Rot *mat.Dense // rotation
	Pos *mat.Dense // translation
}

Replace homogenous matrix with pair (R,p)

func (*Transform) Apply 

func (dst *Transform) Apply(t *Transform)

Update state using the given transformation

func (*Transform) ApplyJoint 

func (dst *Transform) ApplyJoint(tp JointType, q float64)

Apply joint transformation

func (*Transform) Reset 

func (t *Transform) Reset()

Set R = I and p = 0

func (*Transform) Set 

func (t *Transform) Set(src *Transform)

Copy Transform object

Source Files

View all Source files

Jump to

Keyboard shortcuts

? : This menu
/ : Search site
f or F : Jump to
y or Y : Canonical URL
go.dev uses cookies from Google to deliver and enhance the quality of its services and to analyze traffic. Learn more.