Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Paul Dandignac
sot-torque-control
Commits
a80b2418
Commit
a80b2418
authored
Oct 22, 2018
by
Paul Dandignac
Browse files
Rename different task states
parent
024ef4e8
Pipeline
#1753
passed with stage
in 27 minutes and 5 seconds
Changes
2
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
include/sot/torque_control/inverse-dynamics-balance-controller.hh
View file @
a80b2418
...
...
@@ -228,17 +228,17 @@ namespace dynamicgraph {
enum
RightHandState
{
HIGH
_RIGHT_HAND
=
0
,
/*
HIGH
_RIGHT_HAND_TRANSITION = 1,*/
LOW
_RIGHT_HAND
=
1
TASK
_RIGHT_HAND
_ON
=
0
,
/*
TASK
_RIGHT_HAND_TRANSITION = 1,*/
TASK
_RIGHT_HAND
_OFF
=
1
};
RightHandState
m_rightHandState
;
enum
LeftHandState
{
HIGH
_LEFT_HAND
=
0
,
/*
HIGH
_LEFT_HAND_TRANSITION = 1,*/
LOW
_LEFT_HAND
=
1
TASK
_LEFT_HAND
_ON
=
0
,
/*
TASK
_LEFT_HAND_TRANSITION = 1,*/
TASK
_LEFT_HAND
_OFF
=
1
};
LeftHandState
m_leftHandState
;
/*double m_handsTransitionTime;*/
/// end time of the current transition (if any)
...
...
src/inverse-dynamics-balance-controller.cpp
View file @
a80b2418
...
...
@@ -319,8 +319,8 @@ namespace dynamicgraph
,
m_firstTime
(
true
)
,
m_timeLast
(
0
)
,
m_contactState
(
DOUBLE_SUPPORT
)
,
m_rightHandState
(
LOW
_RIGHT_HAND
)
,
m_leftHandState
(
LOW
_LEFT_HAND
)
,
m_rightHandState
(
TASK
_RIGHT_HAND
_OFF
)
,
m_leftHandState
(
TASK
_LEFT_HAND
_OFF
)
,
m_robot_util
(
RefVoidRobotUtil
())
{
RESETDEBUG5
();
...
...
@@ -430,7 +430,7 @@ namespace dynamicgraph
void
InverseDynamicsBalanceController
::
addTaskRightHand
(
/*const double& transitionTime*/
)
{
if
(
m_rightHandState
==
LOW
_RIGHT_HAND
)
if
(
m_rightHandState
==
TASK
_RIGHT_HAND
_OFF
)
{
SEND_MSG
(
"Adds right hand SE3 task in "
/*+toString(transitionTime)+" s"*/
,
MSG_TYPE_INFO
);
const
double
&
w_hands
=
m_w_handsSIN
.
accessCopy
();
...
...
@@ -438,17 +438,17 @@ namespace dynamicgraph
}
/*if(transitionTime>m_dt)
{
m_rightHandState =
HIGH
_RIGHT_HAND_TRANSITION;
m_rightHandState =
TASK
_RIGHT_HAND_TRANSITION;
m_handsTransitionTime = m_t + transitionTime;
}
else
m_rightHandState =
HIGH
_RIGHT_HAND;*/
m_rightHandState
=
HIGH
_RIGHT_HAND
;
m_rightHandState =
TASK
_RIGHT_HAND
_ON
;*/
m_rightHandState
=
TASK
_RIGHT_HAND
_ON
;
}
void
InverseDynamicsBalanceController
::
addTaskLeftHand
(
/*const double& transitionTime*/
)
{
if
(
m_leftHandState
==
LOW
_LEFT_HAND
)
if
(
m_leftHandState
==
TASK
_LEFT_HAND
_OFF
)
{
SEND_MSG
(
"Raise left hand in "
/*+toString(transitionTime)+" s"*/
,
MSG_TYPE_INFO
);
const
double
&
w_hands
=
m_w_handsSIN
.
accessCopy
();
...
...
@@ -456,12 +456,12 @@ namespace dynamicgraph
}
/*if(transitionTime>m_dt)
{
m_leftHandState =
HIGH
_LEFT_HAND_TRANSITION;
m_leftHandState =
TASK
_LEFT_HAND_TRANSITION;
m_handsTransitionTime = m_t + transitionTime;
}
else
m_leftHandState =
HIGH
_LEFT_HAND;*/
m_leftHandState
=
HIGH
_LEFT_HAND
;
m_leftHandState =
TASK
_LEFT_HAND
_ON
;*/
m_leftHandState
=
TASK
_LEFT_HAND
_ON
;
}
void
InverseDynamicsBalanceController
::
addRightFootContact
(
const
double
&
transitionTime
)
...
...
@@ -488,21 +488,21 @@ namespace dynamicgraph
void
InverseDynamicsBalanceController
::
removeTaskRightHand
(
const
double
&
transitionTime
)
{
if
(
m_rightHandState
==
HIGH
_RIGHT_HAND
)
if
(
m_rightHandState
==
TASK
_RIGHT_HAND
_ON
)
{
SEND_MSG
(
"Removes right hand SE3 task in "
+
toString
(
transitionTime
)
+
" s"
,
MSG_TYPE_INFO
);
m_invDyn
->
removeTask
(
m_taskRH
->
name
(),
transitionTime
);
m_rightHandState
=
LOW
_RIGHT_HAND
;
m_rightHandState
=
TASK
_RIGHT_HAND
_OFF
;
}
}
void
InverseDynamicsBalanceController
::
removeTaskLeftHand
(
const
double
&
transitionTime
)
{
if
(
m_leftHandState
==
HIGH
_LEFT_HAND
)
if
(
m_leftHandState
==
TASK
_LEFT_HAND
_ON
)
{
SEND_MSG
(
"Removes left hand SE3 task in "
+
toString
(
transitionTime
)
+
" s"
,
MSG_TYPE_INFO
);
m_invDyn
->
removeTask
(
m_taskLH
->
name
(),
transitionTime
);
m_leftHandState
=
LOW
_LEFT_HAND
;
m_leftHandState
=
TASK
_LEFT_HAND
_OFF
;
}
}
...
...
@@ -753,13 +753,13 @@ namespace dynamicgraph
{
m_contactState
=
LEFT_SUPPORT
;
}
/*if(m_rightHandState ==
HIGH
_RIGHT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
/*if(m_rightHandState ==
TASK
_RIGHT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
{
m_rightHandState =
HIGH
_RIGHT_HAND;
m_rightHandState =
TASK
_RIGHT_HAND
_ON
;
}
if(m_leftHandState ==
HIGH
_LEFT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
if(m_leftHandState ==
TASK
_LEFT_HAND_TRANSITION && m_t >= m_handsTransitionTime)
{
m_leftHandState =
HIGH
_LEFT_HAND;
m_leftHandState =
TASK
_LEFT_HAND
_ON
;
}*/
getProfiler
().
start
(
PROFILE_READ_INPUT_SIGNALS
);
...
...
@@ -826,7 +826,7 @@ namespace dynamicgraph
m_taskLF
->
Kp
(
kp_feet
);
m_taskLF
->
Kd
(
kd_feet
);
}
if
(
m_rightHandState
==
HIGH
_RIGHT_HAND
/*|| m_rightHandState ==
HIGH
_RIGHT_HAND_TRANSITION*/
)
if
(
m_rightHandState
==
TASK
_RIGHT_HAND
_ON
/*|| m_rightHandState ==
TASK
_RIGHT_HAND_TRANSITION*/
)
{
const
Eigen
::
Matrix
<
double
,
12
,
1
>&
x_rh_ref
=
m_rh_ref_posSIN
(
iter
);
const
Vector6
&
dx_rh_ref
=
m_rh_ref_velSIN
(
iter
);
...
...
@@ -840,7 +840,7 @@ namespace dynamicgraph
m_taskRH
->
Kp
(
kp_hands
);
m_taskRH
->
Kd
(
kd_hands
);
}
if
(
m_leftHandState
==
HIGH
_LEFT_HAND
/*|| m_leftHandState ==
HIGH
_LEFT_HAND_TRANSITION*/
)
if
(
m_leftHandState
==
TASK
_LEFT_HAND
_ON
/*|| m_leftHandState ==
TASK
_LEFT_HAND_TRANSITION*/
)
{
const
Eigen
::
Matrix
<
double
,
12
,
1
>&
x_lh_ref
=
m_lh_ref_posSIN
(
iter
);
const
Vector6
&
dx_lh_ref
=
m_lh_ref_velSIN
(
iter
);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment