|
|
joint.h File ReferenceDefinition of the Tjoint type and the associated functions. More... #include "interval.h" #include "link.h" #include "vector.h" #include "equation.h" #include "cuiksystem.h" #include "box.h" #include "plot3d.h" #include "htransform.h" #include "color.h" #include <stdlib.h> #include "boolean.h" Go to the source code of this file.
Detailed DescriptionDefinition of the Tjoint type and the associated functions. Definition in file joint.h. Define Documentation
One of the possible type of joints. A free joint: imposes no constraint between two links. Used to define free flying robots.
Definition at line 31 of file joint.h. Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetJointDOF(), and NewFreeJoint().
One of the possible type of joints. A fix joint: fixes the translation and the orientation of one link with respect to another link using an homogeneous transform.
Definition at line 42 of file joint.h. Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetJointDOF(), GetJointTransform(), and NewFixJoint().
One of the possible type of joints. A revolute joint.
Definition at line 50 of file joint.h. Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetJointDOF(), IsRevoluteBinaryLink(), and NewRevoluteJoint().
One of the possible type of joints. An universal joint.
Definition at line 58 of file joint.h. Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetJointDOF(), and NewUniversalJoint().
One of the possible type of joints. A spherical joint.
Definition at line 66 of file joint.h. Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetJointDOF(), and NewSphericalJoint().
One of the possible type of joints. A spherical-spherical composite joint. This type of joint define a cylinder of a given length between the two links connected by this joint. A single vector represents this cylinder (gives its orientation w.r.t. the global frame). The position is deduced from the anchor point of the link and the rotation around the cylinder is not taken into account. By using a spherical-spherical composite joint we do not have to represent a full reference frame for the cylindre connecting the links and we save variables and equations.
Definition at line 82 of file joint.h. Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetJointDOF(), GetJointLength(), GetJointMaxCoordinate(), MoveJoint(), NewSphSphJoint(), and PlotJoint().
One of the possible type of joints. A prismatic joint.
Definition at line 90 of file joint.h. Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetJointDOF(), GetJointMaxCoordinate(), and NewPrismaticJoint().
Defines a generalized plannar joint where a point in the first link is constrainted to be inside a patch in the second link and the normal of the first link at the given point is aligned with the normal of the patch. The patch is defined as a order-one Bezier patch that is given by 4 points. If those for points are co-planar this joint is a plannar joint with a plane limited by the 4 given points.
Definition at line 105 of file joint.h. Referenced by GenerateJointEquations(), GenerateJointEquationsInBranch(), GenerateJointRangeEquations(), GetJointDOF(), NewInPatchJoint(), and RegenerateJointSolution(). Function Documentation
Defines a free joint between links. A free joint is a dummy joint imposing no constraint between the relative movements of the links. It is used to see a different free flying mechanisms as a single mechanism. This basically simplify the structures we use (we don't have to implement a multi-mechanism stucture, for instance) and the generation of equations from the mechanism definition can be easily implemented with a single ground link. Up to now we hardly used/tested this type of joints.
Definition at line 19 of file joint.c. References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Tjoint::color, Error(), FALSE, FREE_JOINT, Tjoint::hasLimits, HTransformIdentity(), Tjoint::id, INF, Tjoint::length, Tjoint::link, Tjoint::linkID, NewColor(), NewInterval(), NO_UINT, Tjoint::normals, Tjoint::obj3d, Tjoint::points, Tjoint::rad, Tjoint::range, Tjoint::range2, Tjoint::t, and Tjoint::trs. Referenced by GenerateKinTree().
Here is the call graph for this function:
Here is the caller graph for this function:
Defines a fix joint between links. A fix joint fixes the relative translation and orientation between two links. Fix joints are typically used when placing the end effector of a robot to compute its inverse kinematics.
Definition at line 64 of file joint.c. References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Tjoint::color, Error(), FALSE, FIX_JOINT, Tjoint::hasLimits, HTransformCopy(), HTransformGetElement(), Tjoint::id, INF, Tjoint::length, Tjoint::link, Tjoint::linkID, NewColor(), NewInterval(), NO_UINT, Tjoint::normals, Tjoint::obj3d, Tjoint::points, Tjoint::rad, Tjoint::range, Tjoint::range2, Tjoint::t, and Tjoint::trs.
Here is the call graph for this function:
Defines a new revolute joint between two links. Note that we assume centered rotations, that is, if there is any limit in the rotation it is always in the for of a symmetric interval [-range,range]. This is completely general (by defining appropriate reference vectors the rotation range can be made symmetric around 0 and it helps when defining constraints.
Definition at line 120 of file joint.c. References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Tjoint::color, CopyInterval(), Error(), FALSE, Tjoint::hasLimits, HTransformApply(), HTransformDelete(), HTransformIdentity(), HTransformInverse(), HTransformProduct(), HTransformRx(), HTransformX2Vect(), Tjoint::id, INF, IntervalCenter(), IntervalOffset(), Tjoint::length, Tjoint::link, Tjoint::linkID, NewColor(), NewInterval(), NO_UINT, Tjoint::normals, Tjoint::obj3d, Tjoint::points, Tjoint::rad, Tjoint::range, Tjoint::range2, REV_JOINT, Tjoint::t, Tjoint::trs, Tjoint::vrange, and ZERO.
Here is the call graph for this function:
Defines a new universal joint between two links.
Definition at line 250 of file joint.c. References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Tjoint::color, CopyInterval(), Error(), FALSE, Tjoint::hasLimits, HTransformApply(), HTransformDelete(), HTransformIdentity(), HTransformInverse(), HTransformProduct(), HTransformRx(), HTransformX2Vect(), Tjoint::id, INF, IntervalCenter(), IntervalOffset(), Tjoint::length, Tjoint::link, Tjoint::linkID, NewColor(), NewInterval(), NO_UINT, Tjoint::normals, Tjoint::obj3d, Tjoint::points, Tjoint::rad, Tjoint::range, Tjoint::range2, Tjoint::t, Tjoint::trs, UNV_JOINT, Tjoint::vrange, and ZERO.
Here is the call graph for this function:
Defines a new spherical joint between two links. The limits for a spherical joint are defined symmetrically around an axis rigidly linked to the first link.
Definition at line 394 of file joint.c. References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Tjoint::color, Error(), FALSE, Tjoint::hasLimits, HTransformIdentity(), Tjoint::id, INF, Tjoint::length, Tjoint::link, Tjoint::linkID, NewColor(), NewInterval(), NO_UINT, Tjoint::obj3d, Tjoint::points, Tjoint::rad, Tjoint::range, Tjoint::range2, SPH_JOINT, Tjoint::t, Tjoint::trs, and Tjoint::vrange.
Here is the call graph for this function:
Defines a new prismatic joint between two links. Note that prismatic joints are always limited.
Definition at line 473 of file joint.c. References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Tjoint::color, CopyInterval(), Error(), Tjoint::hasLimits, HTransformIdentity(), Tjoint::id, INF, Tjoint::length, Tjoint::link, Tjoint::linkID, NewColor(), NewInterval(), NO_UINT, Tjoint::normals, Tjoint::obj3d, Tjoint::points, PRS_JOINT, Tjoint::rad, Tjoint::range, Tjoint::range2, Tjoint::t, Tjoint::trs, TRUE, and Tjoint::vrange.
Here is the call graph for this function:
Defines a new spherical-spherical composite joint between two links.
Definition at line 536 of file joint.c. References Tjoint::avoidLimits, Tjoint::color, CopyColor(), Error(), FALSE, Tjoint::hasLimits, HTransformIdentity(), Tjoint::id, INF, Tjoint::length, Tjoint::link, Tjoint::linkID, NewInterval(), NO_UINT, Tjoint::obj3d, Tjoint::points, Tjoint::rad, Tjoint::range, Tjoint::range2, SPH_SPH_JOINT, Tjoint::t, Tjoint::trs, and Tjoint::vrange.
Here is the call graph for this function:
Defines a new in-patch joint between two links. An in-patch joint is a generalization of a planar joint where the plane is replaced by a order one Bezier patch. This patch is defined by 4 points. When the four points are coplanar this joint is a planar joint. This type of joints are basically used to define contact between links.
Definition at line 590 of file joint.c. References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, ComputeBoundingBox3d(), CrossProduct(), DotProduct(), Error(), Tjoint::hasLimits, Tjoint::id, IN_PATCH_JOINT, IntervalAdd(), IntervalPow(), IntervalSqrt(), Tjoint::link, Tjoint::linkID, LowerLimit(), Tjoint::normals, Tjoint::normRange, Tjoint::points, Tjoint::t, TRUE, and ZERO.
Here is the call graph for this function:
Defines a joint structure copying data from anther joint.
Definition at line 698 of file joint.c. References Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, Tjoint::color, CopyColor(), CopyInterval(), Tjoint::hasLimits, HTransformCopy(), Tjoint::id, Tjoint::length, Tjoint::link, Tjoint::linkID, Tjoint::normals, Tjoint::normRange, Tjoint::obj3d, Tjoint::points, Tjoint::rad, Tjoint::range, Tjoint::range2, Tjoint::t, Tjoint::trs, and Tjoint::vrange. Referenced by AddJoint2Mechanism().
Here is the call graph for this function:
Here is the caller graph for this function:
Gets the joint type.
Definition at line 747 of file joint.c. References Tjoint::t. Referenced by IsRevoluteBinaryLink().
Here is the caller graph for this function:
Gets the joint identifier (given to the joint when created).
Definition at line 752 of file joint.c. References Tjoint::id.
Gets the identifier of the first joint involved in the link.
Definition at line 757 of file joint.c. References Tjoint::linkID. Referenced by Branch2Link(), GenerateKinTree(), GetBranchDestination(), GetBranchOrigin(), and IsRevoluteBinaryLink().
Here is the caller graph for this function:
Gets a pointer to the first joint involved in the link.
Definition at line 762 of file joint.c. References Tjoint::link.
Gets the identifier of the second joint involved in the link.
Definition at line 767 of file joint.c. References Tjoint::linkID. Referenced by Branch2Link(), GenerateKinTree(), GetBranchDestination(), GetBranchOrigin(), and IsRevoluteBinaryLink().
Here is the caller graph for this function:
Gets a pointer to the second joint involved in the link.
Definition at line 772 of file joint.c. References Tjoint::link.
Gets one of the points defining the rotation/sliding axis for the joint.
Definition at line 777 of file joint.c. References Tjoint::points. Referenced by IsRevoluteBinaryLink().
Here is the caller graph for this function:
Checks if a joint has limits.
Definition at line 787 of file joint.c. References Tjoint::hasLimits. Checks the limits of a joint.
Definition at line 792 of file joint.c. References Tjoint::range. Checks the second limit of a universal joint. Only universal joints have a second range.
Definition at line 797 of file joint.c. References Tjoint::range2.
Computes the degrees of freedom allowed by a given joint. The degrees of freedom constrained by the joint are 6-the allowed ones (i.e., the returned by this function.
Definition at line 802 of file joint.c. References Error(), FIX_JOINT, FREE_JOINT, IN_PATCH_JOINT, PRS_JOINT, REV_JOINT, SPH_JOINT, SPH_SPH_JOINT, Tjoint::t, and UNV_JOINT. Referenced by GetMechanismDOF().
Here is the call graph for this function:
Here is the caller graph for this function:
Returns the distance between the two spherical joints in a spherical-spherical joint. For other types of joints this function triggers and error.
Definition at line 838 of file joint.c. References Error(), Tjoint::length, SPH_SPH_JOINT, and Tjoint::t.
Here is the call graph for this function:
Returns the homogeneous transform defining a fix joint. For other types of joints this function triggers and error.
Definition at line 846 of file joint.c. References Error(), FIX_JOINT, Tjoint::t, and Tjoint::trs.
Here is the call graph for this function:
Adds to the given cuiksystem the variables and equations necessary to deal with the joint ranges. For prismatic joints this is one variable and its associated range. For revolute joints, we have to define two vectors one associted with link1 and the other link2 and define the scalar and vector products (this produces a third vector aligned with the rotation axis) to get the cosinus/sinus of the rotated angle that are then bounded given the range for the allowed rotation between the two links.
Definition at line 854 of file joint.c. References AddCt2Monomial(), AddEquation2CS(), AddMonomial(), AddTerm2SearchCriterion(), AddVariable2CS(), AddVariable2Monomial(), ApplyLinkRot(), Tjoint::avoidLimits, Tjoint::avoidLimitsWeight, COS_VAR, COS_VAR_UNI, DeleteEquation(), DeleteInterval(), DeleteMonomial(), DeleteVariable(), EQU, Error(), FIX_JOINT, FREE_JOINT, FREE_JOINT_VAR, GenerateDotProductEquation(), GetCSVariableID(), GetLinkName(), Tjoint::hasLimits, Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, INF, InitEquation(), InitMonomial(), IntervalCenter(), IntervalCosinus(), IntervalSize(), IsGroundLink, Tjoint::link, Tjoint::linkID, NEW, NewInterval(), NewVariable(), NO_UINT, PRS_JOINT, PRS_JOINT_VAR, Tjoint::range, Tjoint::range2, ResetMonomial(), REV_JOINT, ROT_JOINT_VAR_REF, SECONDARY_VAR, SetEquationCmp(), SetEquationType(), SetEquationValue(), SetVariableInterval(), SPH_JOINT, SPH_JOINT_VAR_REF, SPH_SPH_JOINT, SYSTEM_EQ, SYSTEM_VAR, Tjoint::t, UNV_JOINT, UNV_JOINT_VAR, UNV_JOINT_VAR_REF, and Tjoint::vrange. Referenced by InitWorldKinCS().
Here is the call graph for this function:
Here is the caller graph for this function:
Adds to the given equations the constraints of a joint Before using this method, the variables and equations for individual joints have to be generated (using GenerateJointEquations). When considered in a sequence, joints define a sum of vectors taking from the origin of the frame of reference of one link to the origin of the frame of reference for the next link. When these sums are taken on a closed branch, we get the loop equations. Since links and joints are defined in 3D, summing vectors defined on links and joints define 3 equations, one for X, one for Y, and another for Z.
Definition at line 1090 of file joint.c. References AddCt2Monomial(), AddMonomial(), AddVariable2Monomial(), ApplyLinkRot(), DeleteMonomial(), Error(), FIX_JOINT, FREE_JOINT, FREE_JOINT_VAR, GetCSVariableID(), GetLinkName(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, InitMonomial(), IsGroundLink, Tjoint::length, Tjoint::link, Tjoint::linkID, NEW, NO_UINT, Tjoint::normals, Tjoint::points, PRS_JOINT, PRS_JOINT_VAR, ResetMonomial(), REV_JOINT, SPH_JOINT, SPH_SPH_JOINT, SPH_SPH_JOINT_VAR, Tjoint::t, and UNV_JOINT. Referenced by GenerateTransEquationsFromBranch().
Here is the call graph for this function:
Here is the caller graph for this function:
Adds to the given cuiksystem the variables and equations necessary to deal with the joint. Intutively, we have to add equations so that the revolute/sliding axis is the same as view from link1 and from link2.
Definition at line 1199 of file joint.c. References AddCt2Monomial(), AddEquation2CS(), AddMonomial(), AddVariable2CS(), AddVariable2Monomial(), ApplyLinkRot(), DeleteEquation(), DeleteInterval(), DeleteMonomial(), DeleteVariable(), DUMMY_VAR, EQU, Error(), FIX_JOINT, FREE_JOINT, FREE_JOINT_VAR, GenerateDotProductEquation(), GenerateNormEquation(), GenerateSaddleEquation(), GetLinkName(), HTransformGetElement(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, IN_PATCH_JOINT_SCALE_VAR, InitEquation(), InitMonomial(), IsGroundLink, Tjoint::link, Tjoint::linkID, NEW, NewInterval(), NewVariable(), NO_UINT, Tjoint::normals, Tjoint::normRange, Tjoint::points, PRS_JOINT, ResetMonomial(), REV_JOINT, SECONDARY_VAR, SetEquationCmp(), SetEquationType(), SetVariableInterval(), SPH_JOINT, SPH_SPH_JOINT, SPH_SPH_JOINT_VAR, SYSTEM_EQ, SYSTEM_VAR, Tjoint::t, Tjoint::trs, UNV_JOINT, and UNV_JOINT_VAR. Referenced by InitWorldKinCS().
Here is the call graph for this function:
Here is the caller graph for this function:
Solution points only include values for the system variables (and secondary). However, in some formulations, the frame joint equations use not only system/secondary variables, but dummy variable too. This function computes the values for the joint-related dummy variables form the non-dummy ones for a given joint. Currently only the IN_PATCH_JOINT uses dummy variables but this can change in the future.
Definition at line 1430 of file joint.c. References GetCSVariableID(), GetLinkName(), Tjoint::id, IN_PATCH_JOINT, IN_PATCH_JOINT_CTRL_VAR, Tjoint::link, NEW, and Tjoint::t. Referenced by RegenerateMechanismSolution().
Here is the call graph for this function:
Here is the caller graph for this function:
Returns the maximum coordinate value for all the objects in the joint. Only spherical-spherical joints have an associated object. In this case the function returns the length of the link in between the two spherical joints. Otherwise this function returns 0.
Definition at line 1458 of file joint.c. References Tjoint::length, LowerLimit(), PRS_JOINT, Tjoint::range, SPH_SPH_JOINT, Tjoint::t, and UpperLimit(). Referenced by AddJoint2Mechanism().
Here is the call graph for this function:
Here is the caller graph for this function:
Adds a link to a 3d scene. Nothing is added for all joints but for spherical-spherical joints where we add a cylinder aligned with the X axis.
Definition at line 1483 of file joint.c. References Close3dObject(), Tjoint::color, Tjoint::length, Tjoint::obj3d, PlotCylinder(), PlotSphere(), Tjoint::rad, SPH_SPH_JOINT, StartNew3dObject(), and Tjoint::t. Referenced by PlotMechanism().
Here is the call graph for this function:
Here is the caller graph for this function:
Displaces a joint previously added to a 3d scene. Actually, only in the case of composite joints (spherical-spherical joints) there is an effect for this function. Composite joints (spherical-spherical joints) generate a cylinder connecting the two spherical joints. This function moves this cylinder to the position given by the start-end points of the spherical-spherical joint extracted from the link definitions, the link translation equations, and the link rotation matrixes taken from the given solution point. Note that the rotation around the principal axis of the cylinder does not matters, that is the effect of a spherical-spherical joint.
Definition at line 1501 of file joint.c. References GetTransform2Link(), HTransformApply(), HTransformDelete(), HTransformX2Vect(), IsGroundLink, Tjoint::link, Tjoint::linkID, Move3dObject(), Tjoint::obj3d, Tjoint::points, SPH_SPH_JOINT, and Tjoint::t. Referenced by MoveMechanism().
Here is the call graph for this function:
Here is the caller graph for this function:
Deletes the information stored in a joint and frees the allocated memory.
Definition at line 1545 of file joint.c. References DeleteInterval(), HTransformDelete(), Tjoint::range, Tjoint::range2, and Tjoint::trs. Referenced by DeleteMechanism(), and GenerateKinTree().
Here is the call graph for this function:
Here is the caller graph for this function:
|