Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
140 lines (127 sloc) 5.21 KB
// Accord Math Library
// The Accord.NET Framework
// http://accord-framework.net
//
// Copyright © Rémy Dispagne, 2013
// cramer at libertysurf.fr
//
// Copyright © César Souza, 2009-2017
// cesarsouza at gmail.com
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
namespace Accord.Math.Kinematics
{
using System;
using System.Collections.Generic;
using Accord.Compat;
/// <summary>
/// Denavit-Hartenberg Model Joint.
/// </summary>
///
[Serializable]
public class DenavitHartenbergJoint
{
/// <summary>
/// Gets or sets the current <see cref="DenavitHartenbergMatrix"/> associated with this joint.
/// </summary>
///
public DenavitHartenbergMatrix Matrix { get; private set; }
/// <summary>
/// Gets or sets the position of this joint.
/// </summary>
///
public Vector3 Position { get; set; }
/// <summary>
/// Gets or sets the parameters for this joint.
/// </summary>
///
public DenavitHartenbergParameters Parameters { get; set; }
/// <summary>
/// Initializes a new instance of the <see cref="DenavitHartenbergJoint"/> class.
/// </summary>
///
/// <param name="parameters">The <see cref="DenavitHartenbergParameters">
/// parameters </see> to be used to create the joint.</param>
///
public DenavitHartenbergJoint(DenavitHartenbergParameters parameters)
{
Matrix = new DenavitHartenbergMatrix();
Position = new Vector3(0, 0, 0);
Parameters = parameters;
}
/// <summary>
/// Initializes a new instance of the <see cref="DenavitHartenbergJoint"/> class.
/// </summary>
///
/// <param name="alpha">Angle in radians on the Z axis relatively to the last joint.</param>
/// <param name="theta">Angle in radians on the X axis relatively to the last joint.</param>
/// <param name="radius">Length or radius of the joint.</param>
/// <param name="offset">Offset along Z axis relatively to the last joint.</param>
///
public DenavitHartenbergJoint(double alpha = 0, double theta = 0, double radius = 0, double offset = 0)
: this(new DenavitHartenbergParameters(alpha, theta, radius, offset))
{
}
/// <summary>
/// Updates the joint transformation matrix and position
/// given a model transform matrix and reference position.
/// </summary>
///
public Matrix4x4 Compute(Matrix4x4 transformMatrix, Vector4 referencePosition)
{
// Calculate the new joint
Matrix.Compute(Parameters);
// Calculate the joint transformation matrix relative to the full model
transformMatrix = transformMatrix * Matrix.Transform;
// Calculate the position of the joint as:
// Pn = Z0 * X0 * Z1 * X1 ... Zi * Xi * P0
Position = Matrix4x4.Multiply(transformMatrix, referencePosition).ToVector3();
return transformMatrix; // Return the new matrix, to pass along the chain
}
}
/// <summary>
/// Collection of Denavit Hartenberg Joints.
/// </summary>
///
[Serializable]
public class DenavitHartenbergJointCollection : List<DenavitHartenbergJoint>
{
/// <summary>
/// Adds an object to the end of this <see cref="DenavitHartenbergJointCollection"/>.
/// </summary>
///
/// <param name="parameters">The <see cref="DenavitHartenbergParameters">
/// parameters</see> specifying the joint to be added.</param>
///
public void Add(DenavitHartenbergParameters parameters)
{
Add(parameters);
}
/// <summary>
/// Adds an object to the end of this <see cref="DenavitHartenbergJointCollection"/>.
/// </summary>
///
/// <param name="alpha">Angle in radians on the Z axis relatively to the last joint.</param>
/// <param name="theta">Angle in radians on the X axis relatively to the last joint.</param>
/// <param name="radius">Length or radius of the joint.</param>
/// <param name="offset">Offset along Z axis relatively to the last joint.</param>
///
public void Add(double alpha = 0, double theta = 0, double radius = 0, double offset = 0)
{
Add(new DenavitHartenbergJoint(alpha, theta, radius, offset));
}
}
}
You can’t perform that action at this time.