Skip to content

Commit

Permalink
Merge pull request #75 from gergondet/topic/PostureRefVelAccel
Browse files Browse the repository at this point in the history
PostureTask with reference velocity and acceleration
  • Loading branch information
gergondet authored Jul 15, 2021
2 parents cfc91b5 + 630bcd5 commit 1e408ae
Show file tree
Hide file tree
Showing 8 changed files with 44 additions and 17 deletions.
4 changes: 0 additions & 4 deletions .clang-format
Original file line number Diff line number Diff line change
Expand Up @@ -85,10 +85,6 @@ PenaltyBreakString: 1000
PenaltyExcessCharacter: 150
PenaltyReturnTypeOnItsOwnLine: 300
PointerAlignment: Middle
RawStringFormats:
- Delimiter: pb
Language: TextProto
BasedOnStyle: google
ReflowComments: true
SortIncludes: true
SortUsingDeclarations: true
Expand Down
1 change: 1 addition & 0 deletions .github/workflows/build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@ jobs:
- path: jrl-umi3218/Eigen3ToPython
- path: jrl-umi3218/SpaceVecAlg
- path: jrl-umi3218/sch-core
options: -DCMAKE_CXX_STANDARD=11
- path: jrl-umi3218/eigen-qld
- path: jrl-umi3218/sch-core-python
- path: jrl-umi3218/RBDyn
Expand Down
16 changes: 8 additions & 8 deletions .github/workflows/conan.yml
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,14 @@ name: conan packaging for Tasks
on:
repository_dispatch:
types: [conan-master, conan-release]
push:
branches:
- '**'
tags:
- v*
pull_request:
branches:
- '**'
# push:
# branches:
# - '**'
# tags:
# - v*
# pull_request:
# branches:
# - '**'

jobs:
build:
Expand Down
5 changes: 4 additions & 1 deletion .github/workflows/package.yml
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@ on:
- "**"
tags:
- v*
pull_request:
branches:
- "**"
jobs:
check-tag:
runs-on: ubuntu-18.04
Expand Down Expand Up @@ -230,5 +233,5 @@ jobs:
- name: Trigger dependents rebuild
run: 'curl -H "Accept: application/vnd.github.everest-preview+json" -H "Authorization: token ${{ secrets.GH_PAGES_TOKEN }}" --request POST --data "{\"event_type\": \"${PACKAGE_JOB}\"}" https://api.github.com/repos/jrl-umi3218/mc_rtc/dispatches
'
'
if: env.PACKAGE_UPLOAD == 'true'
3 changes: 3 additions & 0 deletions .github/workflows/sources/package.yml
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,9 @@ on:
- '**'
tags:
- v*
pull_request:
branches:
- '**'

jobs:
# For a given tag vX.Y.Z, this checks:
Expand Down
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ You can also choose the head mirror which will have the latest version of this p

```
curl -1sLf \
'https://dl.cloudsmith.io/public/mc-rtc/stable/setup.deb.sh' \
'https://dl.cloudsmith.io/public/mc-rtc/head/setup.deb.sh' \
| sudo -E bash
```

Expand Down
11 changes: 8 additions & 3 deletions src/QPTasks.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -553,7 +553,7 @@ JointsSelector JointsSelector::UnactiveJoints(
const auto & jDofs = uad.second;
const auto & j = mb.joint(mb.jointIndexByName(jN));
int ji = 0;
for(int i = 0; i < jDofs.size(); ++i)
for(size_t i = 0; i < jDofs.size(); ++i)
{
auto dofStart = jDofs[i][0];
if(dofStart > ji)
Expand Down Expand Up @@ -741,8 +741,11 @@ PostureTask::PostureTask(const std::vector<rbd::MultiBody> & mbs,
double stiffness,
double weight)
: Task(weight), pt_(mbs[rI], q), stiffness_(stiffness), damping_(2. * std::sqrt(stiffness)), robotIndex_(rI),
alphaDBegin_(0), jointDatas_(), Q_(mbs[rI].nrDof(), mbs[rI].nrDof()), C_(mbs[rI].nrDof()), alphaVec_(mbs[rI].nrDof())
alphaDBegin_(0), jointDatas_(), Q_(mbs[rI].nrDof(), mbs[rI].nrDof()), C_(mbs[rI].nrDof()), alphaVec_(mbs[rI].nrDof()),
refVel_(mbs[rI].nrDof()), refAccel_(mbs[rI].nrDof())
{
refVel_.setZero();
refAccel_.setZero();
}

void PostureTask::stiffness(double stiffness)
Expand Down Expand Up @@ -811,7 +814,9 @@ void PostureTask::update(const std::vector<rbd::MultiBody> & mbs,
int deb = mb.jointPosInDof(1);
int end = mb.nrDof() - deb;
// joint
C_.segment(deb, end) = -stiffness_ * pt_.eval().segment(deb, end) + damping_ * alphaVec_.segment(deb, end);
C_.segment(deb, end) = -stiffness_ * pt_.eval().segment(deb, end)
+ damping_ * (alphaVec_.segment(deb, end) - refVel_.segment(deb, end))
- refAccel_.segment(deb, end);

for(const JointData & pjd : jointDatas_)
{
Expand Down
19 changes: 19 additions & 0 deletions src/Tasks/QPTasks.h
Original file line number Diff line number Diff line change
Expand Up @@ -523,6 +523,24 @@ class TASKS_DLLAPI PostureTask : public Task

const Eigen::VectorXd & eval() const;

inline void refVel(const Eigen::VectorXd & refVel) noexcept
{
refVel_ = refVel;
}
inline const Eigen::VectorXd & refVel() const noexcept
{
return refVel_;
}
inline void refAccel(const Eigen::VectorXd & refAccel) noexcept
{
assert(refAccel.size() == refAccel_.size());
refAccel_ = refAccel;
}
inline const Eigen::VectorXd & refAccel() const noexcept
{
return refAccel_;
}

private:
struct JointData
{
Expand All @@ -542,6 +560,7 @@ class TASKS_DLLAPI PostureTask : public Task
Eigen::MatrixXd Q_;
Eigen::VectorXd C_;
Eigen::VectorXd alphaVec_;
Eigen::VectorXd refVel_, refAccel_;
};

class TASKS_DLLAPI PositionTask : public HighLevelTask
Expand Down

0 comments on commit 1e408ae

Please sign in to comment.