server
UtDynamicsSimulator
sDIMS
limit.cpp
Go to the documentation of this file.
1
/*
2
* Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3
* All rights reserved. This program is made available under the terms of the
4
* Eclipse Public License v1.0 which accompanies this distribution, and is
5
* available at http://www.eclipse.org/legal/epl-v10.html
6
* Contributors:
7
* The University of Tokyo
8
*/
14
#include <
ik.h
>
15
16
int
IKScalarJointLimit::calc_jacobian_rotate
(
Joint
* cur)
17
{
18
if
(cur ==
joint
&& cur->
j_type
==
JROTATE
&& cur->
t_given
)
19
{
20
J
(0, cur->
i_dof
) = 1.0;
21
}
22
return
0;
23
}
24
25
int
IKScalarJointLimit::calc_jacobian_slide
(
Joint
* cur)
26
{
27
if
(cur ==
joint
&& cur->
j_type
==
JSLIDE
&& cur->
t_given
)
28
{
29
J
(0, cur->
i_dof
) = 1.0;
30
}
31
return
0;
32
}
33
34
int
IKScalarJointLimit::calc_feedback
()
35
{
36
if
(
active
&&
joint
->
n_dof
== 1)
37
{
38
enabled
=
true
;
39
double
q_cur;
40
joint
->
GetJointValue
(q_cur);
41
if
(
min_limit
&& q_cur <
q_min
)
42
{
43
// cerr << joint->name << " is below min (" << q_cur << " < " << q_min << ")" << endl;
44
fb
(0) =
gain
* (
q_min
- q_cur);
45
}
46
else
if
(
max_limit
&& q_cur >
q_max
)
47
{
48
// cerr << joint->name << " is above max (" << q_cur << " > " << q_max << ")" << endl;
49
fb
(0) =
gain
* (
q_max
- q_cur);
50
}
51
}
52
else
53
{
54
enabled
=
false
;
55
}
56
return
0;
57
}
58
59
void
IKScalarJointLimit::SetCharacterScale
(
double
_scale,
const
char
* charname)
60
{
61
}
62
Joint::t_given
int t_given
torque or motion controlled
Definition:
chain.h:709
IKConstraint::gain
double gain
priority
Definition:
ik.h:396
Joint
The class for representing a joint.
Definition:
chain.h:538
IKScalarJointLimit::q_min
double q_min
Definition:
ik.h:771
Joint::j_type
JointType j_type
joint type
Definition:
chain.h:694
IKScalarJointLimit::calc_jacobian_slide
int calc_jacobian_slide(Joint *cur)
Definition:
limit.cpp:25
IKConstraint::joint
Joint * joint
target joint
Definition:
ik.h:391
IKConstraint::J
fMat J
Definition:
ik.h:402
JROTATE
@ JROTATE
rotational (1DOF)
Definition:
chain.h:41
Joint::n_dof
int n_dof
degrees of freedom (0/1/3/6)
Definition:
chain.h:715
IKScalarJointLimit::calc_jacobian_rotate
int calc_jacobian_rotate(Joint *cur)
Definition:
limit.cpp:16
IKConstraint::active
int active
Definition:
ik.h:400
IKConstraint::enabled
int enabled
number of constraints
Definition:
ik.h:399
IKScalarJointLimit::min_limit
int min_limit
Definition:
ik.h:770
IKScalarJointLimit::calc_feedback
int calc_feedback()
compute the feedback velocity
Definition:
limit.cpp:34
IKScalarJointLimit::SetCharacterScale
void SetCharacterScale(double _scale, const char *charname=0)
Definition:
limit.cpp:59
Joint::GetJointValue
int GetJointValue(double &_q)
Definition:
joint.cpp:256
ik.h
Inverse kinematics (UTPoser) class.
JSLIDE
@ JSLIDE
prismatic (1DOF)
Definition:
chain.h:42
Joint::i_dof
int i_dof
index in all DOF
Definition:
chain.h:720
IKScalarJointLimit::q_max
double q_max
Definition:
ik.h:771
IKConstraint::fb
fVec fb
Jacobian matrix (n_const x total DOF)
Definition:
ik.h:403
IKScalarJointLimit::max_limit
int max_limit
Definition:
ik.h:770
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