kinematics

README

Kinematics

This repo contains the code for the Golang kinematics library, which implements forward and inverse kinematics for robotic arms. We provide default parameters for the AR2 / AR3 robotic arm from Annin Robotics, but other parameters can be used for other arms.

Package documentation can be found at pkg.go.dev or godoc.io

Documentation

Overview

Package kinematics calculates forward and inverse kinematics for robotic arm systems.

Forward kinematics takes joint angles and returns the end effector Pose. Inverse kinematics takes a Pose and returns joint angles that move the end effector to that Pose.

ForwardKinematics (joint angles	-> Pose)
InverseKinematics (Pose	-> joint angles)

Each function also requires a Denavit-Hartenberg Parameter set for the arm of interest. This package provides defaults for the following arm systems:

AR2/AR3

Index

• Variables
• func InverseKinematics(desiredEndEffector Pose, dhParameters DhParameters, thetasInit []float64) ([]float64, error)
• func RandTheta() float64
• type DhParameters
• type Pose
  • func ForwardKinematics(thetas []float64, dhParameters DhParameters) Pose
• type Position
• type Quaternion

Examples

• ForwardKinematics
• InverseKinematics

Variables

var MaxInverseKinematicIteration int = 50

MaxInverseKinematicIteration is the max number of times InverseKinematics should try new seeds before failing. 50 is used here because it is approximately the number of iterations that will take 1 second to compute.

Functions

func InverseKinematics

func InverseKinematics(desiredEndEffector Pose, dhParameters DhParameters,
	thetasInit []float64) ([]float64, error)

InverseKinematics calculates joint angles to achieve a desired end effector Pose, robotic arm DH parameters and the intial joint angles.

Example

package main

import (
	"fmt"
	"github.com/trilobio/kinematics"
)

func main() {
	// Establish the original joint angles
	thetasInit := []float64{0, 0, 0, 0, 0, 0}

	// Establish coordinates to go to
	coordinates := kinematics.Pose{Position: kinematics.Position{X: -100, Y: 250, Z: 250}, Rotation: kinematics.Quaternion{W: 0.41903052745255764, X: 0.4007833787652043, Y: -0.021233218878182854, Z: 0.9086418268616911}}

	// Run kinematics procedure
	angles, _ := kinematics.InverseKinematics(coordinates, kinematics.SixDOFDhParameters, thetasInit)

	// Math works slightly differently on arm and x86 machines when calculating
	// inverse kinematics. We check 5 decimals deep, since it appears numbers can
	// have slight variations between arm and x86 at 6 decimals.
	fmt.Printf("%5f, %5f, %5f, %5f, %5f, %5f\n", angles[0], angles[1], angles[2], angles[3], angles[4], angles[5])
}

Output:

1.846274, 0.341672, -2.313720, -1.776501, 2.221810, 1.231879

func RandTheta

func RandTheta() float64

RandTheta creates a random value from -pi to pi

Types

type DhParameters

type DhParameters struct {
	ThetaOffsets []float64
	AlphaValues  []float64
	AValues      []float64
	DValues      []float64
}

DhParameters stand for "Denavit-Hartenberg Parameters". These parameters define a robotic arm for input into forward or reverse kinematics.

var SevenDOFDhParameters DhParameters = DhParameters{
	ThetaOffsets: []float64{0, 0, 0, 0, 0, 0, 0},
	AlphaValues:  []float64{-math.Pi / 2, math.Pi / 2, -math.Pi / 2, math.Pi / 2, -math.Pi / 2, 0, 0},
	AValues:      []float64{0, 200, 250, 300, 200, 200, 100},
	DValues:      []float64{500, 0, 0, 0, 0, 0, 0},
}

Denavit-Hartenberg Parameters of a ficticious 7 Degress-Of-Freedom arm

var SixDOFDhParameters DhParameters = DhParameters{
	ThetaOffsets: []float64{0, 0, -math.Pi / 2, 0, 0, math.Pi},
	AlphaValues:  []float64{-(math.Pi / 2), 0, math.Pi / 2, -(math.Pi / 2), math.Pi / 2, 0},
	AValues:      []float64{64.2, 305, 0, 0, 0, 0},
	DValues:      []float64{169.77, 0, 0, -222.63, 0, -36.25},
}

Denavit-Hartenberg Parameters of AR3 provided by AR2 Version 2.0 software executable files from https://www.anninrobotics.com/downloads Those parameters are the same between the AR2 and AR3. We use these as an example for a 6 Degrees-Of-Freedom arm.

type Pose

type Pose struct {
	Position Position
	Rotation Quaternion
}

Pose represents a position and rotation, where Position is the translational component, and Rot is the quaternion representing the rotation.

func ForwardKinematics

func ForwardKinematics(thetas []float64, dhParameters DhParameters) Pose

ForwardKinematics calculates the end effector Pose coordinates given joint angles and robotic arm parameters.

Example

package main

import (
	"fmt"
	"github.com/trilobio/kinematics"
)

func main() {
	angles := []float64{10, 1, 1, 0, 0, 0}
	coordinates := kinematics.ForwardKinematics(angles, kinematics.SixDOFDhParameters)

	fmt.Println(coordinates)
	// Output {{-101.74590611879692 -65.96805988175777 -322.27756822304093} {0.06040824945687102 -0.20421099379003957 0.2771553334491873 0.9369277637862541}}
}

type Position

type Position struct {
	X float64
	Y float64
	Z float64
}

Position represents a position in 3D cartesian space.

type Quaternion

type Quaternion struct {
	W float64
	X float64
	Y float64
	Z float64
}