Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
uam_task_ctrl
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Pep Martí Saumell
uam_task_ctrl
Commits
a60c822c
Commit
a60c822c
authored
9 years ago
by
asantamaria
Browse files
Options
Downloads
Patches
Plain Diff
debugging. till line 435
parent
ac1da1ca
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/quadarm_task_priority_ctrl.cpp
+30
-23
30 additions, 23 deletions
src/quadarm_task_priority_ctrl.cpp
src/quadarm_task_priority_ctrl.h
+3
-1
3 additions, 1 deletion
src/quadarm_task_priority_ctrl.h
with
33 additions
and
24 deletions
src/quadarm_task_priority_ctrl.cpp
+
30
−
23
View file @
a60c822c
...
@@ -17,7 +17,7 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
...
@@ -17,7 +17,7 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
void
CQuadarm_Task_Priority_Ctrl
::
quadarm_task_priority_ctrl
(
const
double
&
quad_height
,
void
CQuadarm_Task_Priority_Ctrl
::
quadarm_task_priority_ctrl
(
const
double
&
quad_height
,
const
goal_obj
&
goal
,
const
goal_obj
&
goal
,
ht
&
HT
,
ht
&
HT
,
const
arm
&
arm
,
arm
&
arm
,
const
quadrotor
&
quad
,
const
quadrotor
&
quad
,
ctrl_params
&
ctrl_params
,
ctrl_params
&
ctrl_params
,
MatrixXd
&
robot_pos
,
MatrixXd
&
robot_pos
,
...
@@ -52,7 +52,8 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
...
@@ -52,7 +52,8 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const double& quad_
this
->
quad_
.
pos
=
this
->
quad_
.
pos
+
(
this
->
uam_
.
vel
.
block
(
0
,
0
,
6
,
1
)
*
this
->
ctrl_params_
.
dt
);
this
->
quad_
.
pos
=
this
->
quad_
.
pos
+
(
this
->
uam_
.
vel
.
block
(
0
,
0
,
6
,
1
)
*
this
->
ctrl_params_
.
dt
);
// return values
// return values
HT
=
this
->
T_
;
HT
=
this
->
T_
;
// DEBUG. TODO: make it constant and do not use this trans. outside.
arm
.
T_base_to_joint
=
this
->
arm_
.
T_base_to_joint
;
// DEBUG. TODO: make it constant and do not use this trans. outside.
ctrl_params
=
this
->
ctrl_params_
;
ctrl_params
=
this
->
ctrl_params_
;
robot_pos
.
block
(
0
,
0
,
6
,
1
)
=
this
->
quad_
.
pos
;
robot_pos
.
block
(
0
,
0
,
6
,
1
)
=
this
->
quad_
.
pos
;
robot_pos
.
block
(
6
,
0
,
this
->
arm_
.
nj
,
1
)
=
this
->
arm_
.
jnt_pos
;
robot_pos
.
block
(
6
,
0
,
this
->
arm_
.
nj
,
1
)
=
this
->
arm_
.
jnt_pos
;
...
@@ -375,7 +376,6 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
...
@@ -375,7 +376,6 @@ void CQuadarm_Task_Priority_Ctrl::task_vs(MatrixXd& JVS,MatrixXd& JVS_pseudo,Mat
void
CQuadarm_Task_Priority_Ctrl
::
task_cog
(
MatrixXd
&
JG
,
MatrixXd
&
JG_pseudo
,
MatrixXd
&
sigmaG
)
void
CQuadarm_Task_Priority_Ctrl
::
task_cog
(
MatrixXd
&
JG
,
MatrixXd
&
JG_pseudo
,
MatrixXd
&
sigmaG
)
{
{
// Initializations
// Initializations
MatrixXd
qa
;
// joint positions
this
->
arm_
.
mass
=
0
;
// Total arm mass
this
->
arm_
.
mass
=
0
;
// Total arm mass
MatrixXd
mass
=
MatrixXd
::
Zero
(
this
->
arm_
.
nj
,
1
);
// Mass of the links
MatrixXd
mass
=
MatrixXd
::
Zero
(
this
->
arm_
.
nj
,
1
);
// Mass of the links
this
->
ctrl_params_
.
cog_arm
=
MatrixXd
::
Zero
(
3
,
1
);
// Arm CoG
this
->
ctrl_params_
.
cog_arm
=
MatrixXd
::
Zero
(
3
,
1
);
// Arm CoG
...
@@ -403,40 +403,37 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
...
@@ -403,40 +403,37 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
for
(
unsigned
int
ii
=
1
;
ii
<
js
;
++
ii
)
for
(
unsigned
int
ii
=
1
;
ii
<
js
;
++
ii
)
arm_segment
.
at
(
ii
)
=
this
->
arm_
.
chain
.
getSegment
(
ii
);
arm_segment
.
at
(
ii
)
=
this
->
arm_
.
chain
.
getSegment
(
ii
);
// Get joint positions
qa
=
this
->
arm_
.
jnt_pos
;
//Fixed transform from arm_base to base_link
// Matrix4d Tbase = Matrix4d::Zero();
// Tbase(0,0) = -1;
// Tbase(1,1) = 1;
// Tbase(2,2) = -1;
// Tbase(3,3) = 1;
Matrix4d
Tbase
=
Matrix4d
::
Identity
();
// Origin of the base frame
// Origin of the base frame
MatrixXd
pini
(
4
,
1
);
MatrixXd
pini
(
4
,
1
);
pini
.
col
(
0
)
<<
0
,
0
,
0
,
1
;
pini
.
col
(
0
)
<<
0
,
0
,
0
,
1
;
// Initial transform
// Initial transform
T_base_to_joint
.
at
(
0
)
=
Tbase
;
// T_base_to_joint.at(0) = this->T_.armbase_in_baselink;
T_base_to_joint
.
at
(
0
)
=
Matrix4d
::
Identity
();
// 3rd column of rotation matrix
// 3rd column of rotation matrix
IIIrdcolRot_b_x
.
at
(
0
)
=
T_base_to_joint
.
at
(
0
).
block
(
0
,
2
,
3
,
1
);
IIIrdcolRot_b_x
.
at
(
0
)
=
T_base_to_joint
.
at
(
0
).
block
(
0
,
2
,
3
,
1
);
// Origin of the initial frame
// Origin of the initial frame
frame_link
.
at
(
0
)
=
T_base_to_joint
.
at
(
0
)
*
pini
;
frame_link
.
at
(
0
)
=
T_base_to_joint
.
at
(
0
)
*
pini
;
// Debug
// Debug
//std::cout << "**************" << std::endl;
this
->
arm_cog_data_
.
link
.
resize
(
this
->
arm_
.
nj
);
this
->
arm_cog_data_
.
link
.
resize
(
this
->
arm_
.
nj
);
this
->
arm_
.
T_base_to_joint
.
resize
(
this
->
arm_
.
nj
+
1
);
this
->
arm_
.
T_base_to_joint
.
at
(
0
)
=
T_base_to_joint
.
at
(
0
);
std
::
cout
<<
"**************"
<<
std
::
endl
;
for
(
unsigned
int
jj
=
0
;
jj
<
this
->
arm_
.
nj
;
++
jj
)
for
(
unsigned
int
jj
=
0
;
jj
<
this
->
arm_
.
nj
;
++
jj
)
{
{
// Get mass of each link
// Get mass of each link
mass
(
jj
,
0
)
=
arm_segment
.
at
(
jj
+
1
).
getInertia
().
getMass
();
mass
(
jj
,
0
)
=
arm_segment
.
at
(
jj
+
1
).
getInertia
().
getMass
();
// Joint frames
// Joint frames
joint_pose
.
at
(
jj
)
=
arm_segment
.
at
(
jj
+
1
).
pose
(
qa
(
jj
,
0
));
joint_pose
.
at
(
jj
)
=
arm_segment
.
at
(
jj
+
1
).
pose
(
this
->
arm_
.
jnt_pos
(
jj
,
0
));
// relative Homogeneous transforms (HT)
// relative Homogeneous transforms (HT)
Transf
.
at
(
jj
)
=
GetTransform
(
joint_pose
.
at
(
jj
));
Transf
.
at
(
jj
)
=
GetTransform
(
joint_pose
.
at
(
jj
));
//TODO: Debugging till here
// HT r.t. base link
// HT r.t. base link
T_base_to_joint
.
at
(
jj
+
1
)
=
T_base_to_joint
.
at
(
jj
)
*
Transf
.
at
(
jj
);
T_base_to_joint
.
at
(
jj
+
1
)
=
T_base_to_joint
.
at
(
jj
)
*
Transf
.
at
(
jj
);
// 3rd column of rotation matrix
// 3rd column of rotation matrix
...
@@ -461,12 +458,19 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
...
@@ -461,12 +458,19 @@ void CQuadarm_Task_Priority_Ctrl::task_cog(MatrixXd& JG,MatrixXd& JG_pseudo,Matr
// std::cout << frame_link.at(jj)+dist.at(jj) << std::endl;
// std::cout << frame_link.at(jj)+dist.at(jj) << std::endl;
// std::cout << "Transform: " << std::endl;
// std::cout << "Transform: " << std::endl;
// std::cout << T_base_to_joint.at(jj+1) <<std::endl;
// std::cout << T_base_to_joint.at(jj+1) <<std::endl;
std
::
cout
<<
"______joint: "
<<
jj
<<
std
::
endl
;
std
::
cout
<<
Transf
.
at
(
jj
).
block
(
0
,
3
,
3
,
1
)
<<
std
::
endl
;
Matrix3d
Rot
=
Transf
.
at
(
jj
).
block
(
0
,
0
,
3
,
3
);
std
::
cout
<<
Rot
.
eulerAngles
(
0
,
1
,
2
)
<<
std
::
endl
;
// Fill link information
// Fill link information
this
->
arm_cog_data_
.
link
.
at
(
jj
).
origin
=
T_base_to_joint
.
at
(
jj
+
1
);
this
->
arm_cog_data_
.
link
.
at
(
jj
).
origin
=
T_base_to_joint
.
at
(
jj
+
1
);
this
->
arm_cog_data_
.
link
.
at
(
jj
).
cog
=
(
frame_link
.
at
(
jj
+
1
)
-
frame_link
.
at
(
jj
))
/
2
;
this
->
arm_cog_data_
.
link
.
at
(
jj
).
cog
=
(
frame_link
.
at
(
jj
+
1
)
-
frame_link
.
at
(
jj
))
/
2
;
this
->
arm_cog_data_
.
link
.
at
(
jj
).
mass
=
mass
(
jj
,
0
);
this
->
arm_cog_data_
.
link
.
at
(
jj
).
mass
=
mass
(
jj
,
0
);
this
->
arm_
.
T_base_to_joint
.
at
(
jj
+
1
)
=
T_base_to_joint
.
at
(
jj
+
1
);
// this->arm_.T_base_to_joint.at(jj+1) = Transf.at(jj);
}
}
// Debug
// Debug
// std::cout << "mass: " << this->arm_.mass << std::endl;
// std::cout << "mass: " << this->arm_.mass << std::endl;
// std::cout << "arm_cg: " << arm_cg << std::endl;
// std::cout << "arm_cg: " << arm_cg << std::endl;
...
@@ -654,16 +658,19 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
...
@@ -654,16 +658,19 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
Matrix3d
R
;
Matrix3d
R
;
Matrix4d
T
;
Matrix4d
T
;
f
.
M
.
GetEulerZYX
(
yaw
,
pitch
,
roll
);
// f.M.GetEulerZYX(yaw,pitch,roll);
f
.
M
.
GetRPY
(
roll
,
pitch
,
yaw
);
// euler convention zyx
// euler convention zyx
R
=
Eigen
::
AngleAxisd
(
yaw
,
Vector3d
::
Unit
Z
())
\
R
=
Eigen
::
AngleAxisd
(
roll
,
Vector3d
::
Unit
X
())
\
*
Eigen
::
AngleAxisd
(
pitch
,
Vector3d
::
UnitY
())
\
*
Eigen
::
AngleAxisd
(
pitch
,
Vector3d
::
UnitY
())
\
*
Eigen
::
AngleAxisd
(
roll
,
Vector3d
::
Unit
X
());
*
Eigen
::
AngleAxisd
(
yaw
,
Vector3d
::
Unit
Z
());
T
.
block
(
0
,
0
,
3
,
3
)
=
R
;
T
.
block
(
0
,
0
,
3
,
3
)
=
R
;
T
(
0
,
3
)
=
(
double
)
f
.
p
.
data
[
0
];
// Vector3d kdltrans((double)f.p.x(),(double)f.p.y(),(double)f.p.z());
T
(
1
,
3
)
=
(
double
)
f
.
p
.
data
[
1
];
// T.block(0,3,3,1) = R.transpose()*kdltrans;
T
(
2
,
3
)
=
(
double
)
f
.
p
.
data
[
2
];
T
(
0
,
3
)
=
(
double
)
f
.
p
.
x
();
T
(
1
,
3
)
=
(
double
)
f
.
p
.
y
();
T
(
2
,
3
)
=
(
double
)
f
.
p
.
z
();
T
.
row
(
3
)
<<
0
,
0
,
0
,
1
;
T
.
row
(
3
)
<<
0
,
0
,
0
,
1
;
return
T
;
return
T
;
...
...
This diff is collapsed.
Click to expand it.
src/quadarm_task_priority_ctrl.h
+
3
−
1
View file @
a60c822c
...
@@ -48,6 +48,7 @@ typedef struct{
...
@@ -48,6 +48,7 @@ typedef struct{
Matrix4d
cam_in_tip
;
// Camera in arm tip frames.
Matrix4d
cam_in_tip
;
// Camera in arm tip frames.
Matrix4d
tip_in_baselink
;
// Arm forward kinematics (from base_link to arm link frame).
Matrix4d
tip_in_baselink
;
// Arm forward kinematics (from base_link to arm link frame).
Matrix4d
cam_in_baselink
;
// Camera in base_link frames.
Matrix4d
cam_in_baselink
;
// Camera in base_link frames.
Matrix4d
armbase_in_baselink
;
// Camera in base_link frames.
}
ht
;
}
ht
;
// Arm joint definition
// Arm joint definition
...
@@ -69,6 +70,7 @@ typedef struct{
...
@@ -69,6 +70,7 @@ typedef struct{
boost
::
scoped_ptr
<
ChainJntToJacSolver
>
jnt_to_jac_solver
;
// chain solver.
boost
::
scoped_ptr
<
ChainJntToJacSolver
>
jnt_to_jac_solver
;
// chain solver.
JntArray
jnt_pos_kdl
;
// Array of arm positions.
JntArray
jnt_pos_kdl
;
// Array of arm positions.
Jacobian
jacobian
;
// Jacobian.
Jacobian
jacobian
;
// Jacobian.
vector
<
MatrixXd
>
T_base_to_joint
;
// Homogenous Transforms from arm base to each link.
}
arm
;
}
arm
;
// Quadrotor definition
// Quadrotor definition
...
@@ -265,7 +267,7 @@ class CQuadarm_Task_Priority_Ctrl
...
@@ -265,7 +267,7 @@ class CQuadarm_Task_Priority_Ctrl
void
quadarm_task_priority_ctrl
(
const
double
&
quad_height
,
void
quadarm_task_priority_ctrl
(
const
double
&
quad_height
,
const
goal_obj
&
goal
,
const
goal_obj
&
goal
,
ht
&
HT
,
ht
&
HT
,
const
arm
&
arm
,
arm
&
arm
,
const
quadrotor
&
quad
,
const
quadrotor
&
quad
,
ctrl_params
&
ctrl_params
,
ctrl_params
&
ctrl_params
,
MatrixXd
&
robot_pos
,
MatrixXd
&
robot_pos
,
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment