Go to the documentation of this file.
27 #define MIN_JOINT_WEIGHT 0.01
29 #define MAX_CONDITION_NUMBER 100.0
38 :
virtual public Chain
108 double Update(
double timestep);
190 IKHandle*
AddMarker(
const std::string& label,
const std::string& linkname,
const std::string& charname,
const fVec3& rel_pos);
201 int init(SceneGraph* sg);
202 int myinit(SceneGraph* sg);
435 const fVec3& _rel_pos = 0.0,
const fMat33& _rel_att = 1.0,
436 Joint* _other_joint = 0)
439 for(
int i=0;
i<6;
i++)
527 for(
int i=0;
i<6;
i++)
664 charname =
new char [strlen(_charname)+1];
668 for(
int i=0;
i<3;
i++)
void Activate()
activate the constraint
int SetJointWeight(const char *jname, double _weight)
Set joint weight, for single-DOF joints.
IKHandle * AddMarker(const std::string &label, const std::string &linkname, const std::string &charname, const fVec3 &rel_pos)
Add new marker constraint.
void set_character_scale(Joint *jnt, double _scale, const char *charname)
int init(SceneGraph *sg)
Initialize the parameters.
int SetDesireGain(const char *jname, double _gain)
Set gain of desire constraint of the specific joint.
fVec joint_weights
joint weights
fVec3 abs_pos
absolute position
int ResetAllConstraints()
Reset all constraints.
int ConstraintIndex(ConstType _type, const char *jname)
void Diactivate()
diactivate the constraint
void set(const fMat33 &mat)
Copies a matrix.
void SetPos(const fVec3 &_abs_pos)
set the constraint position
double GetMaxConditionNumber()
Get maximum condition number.
int RemoveAllConstraints()
int LoadMarkerXML(const char *fname, const char *_charname)
Load marker information from XML file.
int load_markers(SceneGraph *sg, Joint *rj)
void SetMin(double _q_min)
int AddConstraint(IKConstraint *_constraint)
int edit_marker(IKHandle *marker, const char *body_name, Joint *parent_joint, const fVec3 &abs_pos)
The class for representing a joint.
int calc_feedback()
compute the feedback velocity
void SetRelPos(const fVec3 &_rel_pos)
set relative position of the constraint
@ HIGH_PRIORITY
strictly satisfied
IK::ConstIndex const_index[3]
which of three directions (x, y, z) are constrained
virtual IK::ConstType GetType()=0
Returns the constraint type.
fVec3 des_com
desired COM position
int calc_jacobian_sphere(Joint *cur)
void mul(const fMat33 &mat1, const fMat33 &mat2)
The class representing the whole mechanism. May contain multiple characters.
IKConstraint ** constraints
list of current constraints
int n_constraints
number of current constraints
JointType j_type
joint type
void SetDesire(double _q)
int assign_constraints(int _n)
double q_des
desired joint value for 1DOF joints
IKHandle(IK *_ik, const char *_jname, Joint *_jnt, IK::ConstIndex cindex[6], IK::Priority _pri, double _gain, const fVec3 &_rel_pos=0.0, const fMat33 &_rel_att=1.0, Joint *_other_joint=0)
virtual int calc_jacobian()
Computes the constraint Jacobian.
int calc_jacobian_slide(Joint *cur)
virtual int calc_feedback()=0
compute the feedback velocity
Joint * joint
target joint
@ COM_CONSTRAINT
center of mass position
double Update(double timestep)
IKConstraint * FindConstraint(ConstType _type, const char *jname, const char *charname=0)
double GetGain()
get the gain
void SetMaxConditionNumber(double cn)
Set maximum condition number.
int NumDOF()
Total degrees of freedom.
void SetCharacterScale(double _scale, const char *charname=0)
@ HAVE_CONSTRAINT
with constraint
@ JROTATE
rotational (1DOF)
@ JSPHERE
spherical (3DOF)
int n_dof
degrees of freedom (0/1/3/6)
int RemoveConstraint(int _id)
int Reset()
reset the constraint position/orientation by the current values
Main class for inverse kinematic computation.
int NumConstraints()
Number of constraints.
Joint * other_joint
connected to other joint: constrains the relative position and/or orientation w.r....
fMat J[N_PRIORITY_TYPES]
Jacobian matrix of each type.
void Disable()
disable the constraint
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order.
int resize(int i, int j)
Changes the matrix size.
int is_dropped
index in the constraints with the same priority
void SetAtt(const fMat33 &_abs_att)
set the constraint orientation
IKScalarJointLimit(IK *_ik, const char *_jname, Joint *_jnt, IK::Priority _pri, double _gain)
void zero()
Creates a zero matrix.
int calc_feedback()
compute the feedback velocity
void set(double *v)
Set element values from array or three values.
IKConstraint(IK *_ik, const char *_jname, Joint *_jnt, IK::Priority _pri, double _gain)
Default constructor.
void CalcPosition()
Forward kinematics.
int calc_jacobian_rotate(Joint *cur)
IK::Priority GetPriority()
get the priproty
int calc_jacobian()
Computes the constraint Jacobian.
int id
ID (unique to each constraint)
virtual ~IKConstraint()
Destructor.
int enabled
number of constraints
void SetGain(double _gain)
set the gain
virtual int calc_jacobian_sphere(Joint *cur)
void GetConstIndex(IK::ConstIndex _cindex[])
int calc_feedback()
compute the feedback velocity
void SetDesire(const fMat33 &_att)
void SetCharacterScale(double _scale, const char *charname=0)
IK::ConstIndex const_index[6]
with/without constraint for each DOF
@ LOW_PRIORITY
lower priority
@ NO_CONSTRAINT
without constraint
@ HIGH_IF_POSSIBLE
strictly satisfied if possible
virtual int calc_jacobian_free(Joint *cur)
void SetCharacterScale(double _scale, const char *charname=0)
Set character scale parameter.
@ DESIRE_CONSTRAINT
desired joint values
Classes for defining open/closed kinematic chains.
int GetJointValue(double &_q)
fVec weight[N_PRIORITY_TYPES]
weight of each type
@ HANDLE_CONSTRAINT
position/orientation of link
IKCom(IK *_ik, const char *_charname, IK::ConstIndex cindex[3], IK::Priority _pri, double _gain)
fVec3 pos_des
desired joint position for 6DOF joints
virtual void SetCharacterScale(double _scale, const char *charname=0)
double solve_ik()
compute the joint velocity
IKHandle * add_marker(const char *marker_name, Joint *parent_joint, const fVec3 &abs_pos)
void Enable()
enable the constraint
int ResetConstraints(ConstType t)
Reset the constraints with the specific type.
virtual int calc_jacobian_slide(Joint *cur)
fVec3 abs_pos
current constraint position
int n_const[N_PRIORITY_TYPES]
number of constraints of each type
void SetDesire(const fVec3 &_pos, const fMat33 &_att)
virtual int calc_jacobian_rotate(Joint *cur)
IK::ConstType GetType()
Returns the constraint type.
fMat33 abs_att
absolute orientation
int calc_jacobian_free(Joint *cur)
void sub(const fVec3 &vec1, const fVec3 &vec2)
void zero()
Creates a zero vector.
int EnableDesire(const char *jname)
Enable desire constraint of the specific joint.
IK::ConstType GetType()
Returns the constraint type.
IK::ConstType GetType()
Returns the constraint type.
int EditMarker(IKHandle *marker, const char *body_name, Joint *parent_joint, const fVec3 &abs_pos=0.0)
int n_total_const
number of total constraints used so far, including removed ones
IK::ConstType GetType()
Returns the constraint type.
void resize(int i)
Change the size.
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
fMat33 abs_att
current constraint orientation
void SetPos(const fVec3 &p)
void SetMax(double _q_max)
static const char joint_name_separator
int calc_jacobian()
Computes the constraint Jacobian.
int calc_jacobian_rotate(Joint *cur)
void GetAtt(fMat33 &_abs_att)
obtain constraint orientation
@ SCALAR_JOINT_LIMIT_CONSTRAINT
joint limit for rotate and slide joints
int calc_feedback()
compute the feedback velocity
fVec3 cur_com
current COM position
void SetPriority(IK::Priority _pri)
set the priproty
int calc_jacobian()
compute the Jacobian matrix
void GetPos(fVec3 &_abs_pos)
obtain constraint position
int Active()
whether the constraint is activate
void identity()
Identity matrix.
IKDesire(IK *_ik, const char *_jname, Joint *_jnt, IK::Priority _pri, double _gain)
int save_marker(TransformNode *top_tnode, IKHandle *h)
void SetDesire(const fVec3 &_pos)
int myinit(SceneGraph *sg)
#define DEFAULT_MAX_INTEG_ERROR
@ IK_NUM_CONSTRAINT_TYPES
Base class for constraints.
int n_assigned_constraints
void SetConstraintScale(double _scale, const char *charname=0)
Set constraint scale parameter.
int i_const
feedback velocity (n_const)
int SaveMarkers(const char *fname)
fMat33 att_des
desired joint orientation for 3/6 DOF joints
fVec fb[N_PRIORITY_TYPES]
constraint error of each type
char * charname
target character name
int DisableDesire(const char *jname)
Disable desire constraint of the specific joint.
void SetCharacterScale(double _scale, const char *charname=0)
int ConstraintID(ConstType _type, const char *jname)
fVec fb
Jacobian matrix (n_const x total DOF)
int copy_jacobian()
copy each constraint Jacobian to the whole Jacobian matrix
void zero()
Creates a zero vector.
double max_condnum
maximum condition number
void mul(const fVec3 &vec, double d)
int calc_jacobian_slide(Joint *cur)
Joint limit constraint for 1-DOF joints.
double TotalCOM(fVec3 &com, const char *chname=0)
Center of mass of the chain.
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03