Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
M
master project
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
Package registry
Container registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
GitLab community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Joschua Gosda
master project
Commits
cb132a06
Commit
cb132a06
authored
3 years ago
by
Joschua Gosda
Browse files
Options
Downloads
Patches
Plain Diff
cleared all pointers from function arguments
parent
66780283
Branches
Branches containing commit
No related tags found
1 merge request
!2
Cleanup
Changes
4
Show whitespace changes
Inline
Side-by-side
Showing
4 changed files
c++/include/yumi.hpp
+1
-1
1 addition, 1 deletion
c++/include/yumi.hpp
c++/src/main.cpp
+76
-0
76 additions, 0 deletions
c++/src/main.cpp
c++/src/yumi.cpp
+8
-8
8 additions, 8 deletions
c++/src/yumi.cpp
python/taskspace_placement/computeJoints.py
+2
-2
2 additions, 2 deletions
python/taskspace_placement/computeJoints.py
with
87 additions
and
11 deletions
c++/include/yumi.hpp
+
1
−
1
View file @
cb132a06
...
...
@@ -59,7 +59,7 @@ class Yumi {
Eigen
::
Matrix
<
double
,
7
,
1
>
m_jointAnglesDelta
;
// private functions
void
doForwardKinematics
(
rl
::
mdl
::
Kinematic
*
kinematic
);
void
doForwardKinematics
();
Eigen
::
Matrix3d
euler2rotMatrix
(
rl
::
math
::
Vector3
orientation
);
void
compTaskSpaceInput
();
...
...
This diff is collapsed.
Click to expand it.
c++/src/main.cpp
0 → 100644
+
76
−
0
View file @
cb132a06
#include
<iostream>
#include
"yumi.hpp"
#include
<Eigen/Eigen>
#include
<rl/math/Unit.h>
#include
<rl/mdl/Kinematic.h>
#include
<rl/mdl/Model.h>
#include
<rl/mdl/UrdfFactory.h>
int
main
(
int
,
char
**
)
{
std
::
string
path2yumi_l
=
"/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf"
;
Yumi
yumi_l
(
path2yumi_l
);
Eigen
::
Matrix
<
double
,
7
,
1
>
jointAngles
;
jointAngles
<<
90.48
,
17.87
,
-
25.0
,
48.0
,
-
137.0
,
122.0
,
-
74.21
;
jointAngles
*=
rl
::
math
::
DEG2RAD
;
Eigen
::
Matrix
<
double
,
7
,
1
>
jointVelocities
;
jointVelocities
<<
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
;
yumi_l
.
set_jointValues
(
jointAngles
,
jointVelocities
);
// Desired Values
Eigen
::
Matrix
<
double
,
6
,
1
>
desPose
;
desPose
<<
0.300
,
0.200
,
0.200
,
0.0
*
rl
::
math
::
DEG2RAD
,
0.0
*
rl
::
math
::
DEG2RAD
,
0.0
*
rl
::
math
::
DEG2RAD
;
// obtained from RS with stated joint values
Eigen
::
Matrix
<
double
,
6
,
1
>
desVelocity
;
desVelocity
<<
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
;
yumi_l
.
set_desPoseVel
(
desPose
,
desVelocity
);
yumi_l
.
process
();
Eigen
::
Matrix
<
double
,
7
,
1
>
newJointValues
;
newJointValues
=
yumi_l
.
get_newJointValues
();
std
::
cout
<<
"old joint values: "
<<
jointAngles
<<
std
::
endl
;
std
::
cout
<<
"new joint values: "
<<
newJointValues
<<
std
::
endl
;
//enum yumi_arm{YUMI_RIGHT, YUMI_LEFT};
/* rl::mdl::UrdfFactory factory;
std::shared_ptr<rl::mdl::Model> model(factory.create("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf"));
rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
// Is Values
Eigen::Matrix<double, 6, 1> actualPosition;
Eigen::Matrix<double, 7, 1> jointAngles;
//jointAngles << 90.48, 17.87, -25.0, 48.0, -137.0, 122.0, -74.21; // start position left arm, angles from RS
jointAngles << -110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0; // start position right arm, angles from RS
//jointAngles << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
jointAngles *= rl::math::DEG2RAD;
Eigen::Matrix<double, 7, 1> jointVelocity;
jointVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
// Desired Values
Eigen::Matrix<double, 6, 1> desPose;
desPose << 0.300, 0.200, 0.200, 33.4*rl::math::DEG2RAD, 157.0*rl::math::DEG2RAD, 39.4*rl::math::DEG2RAD; // obtained from RS with stated joint values
Eigen::Matrix<double, 6, 1> desVelocity;
desVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> result;
result = gpm(desPose, desVelocity, jointAngles, jointVelocity, kinematic);
std::cout << "desired joint values: \n" << result.first << std::endl;
std::cout << "current pose: \n" << result.second << std::endl; */
return
0
;
}
This diff is collapsed.
Click to expand it.
c++/src/yumi.cpp
+
8
−
8
View file @
cb132a06
...
...
@@ -24,17 +24,17 @@ void Yumi::set_jointValues(Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matr
m_jointVelocity
=
jointVelocity
;
}
void
Yumi
::
doForwardKinematics
(
rl
::
mdl
::
Kinematic
*
kinematic
){
kinematic
->
setPosition
(
m_jointAngles
);
kinematic
->
forwardPosition
();
kinematic
->
calculateJacobian
();
rl
::
math
::
Transform
t
=
kinematic
->
getOperationalPosition
(
0
);
void
Yumi
::
doForwardKinematics
(){
m_
kinematic
->
setPosition
(
m_jointAngles
);
m_
kinematic
->
forwardPosition
();
m_
kinematic
->
calculateJacobian
();
rl
::
math
::
Transform
t
=
m_
kinematic
->
getOperationalPosition
(
0
);
m_position
=
t
.
translation
();
m_orientation
=
t
.
rotation
().
eulerAngles
(
2
,
1
,
0
).
reverse
();
}
void
Yumi
::
print_pose
(){
Yumi
::
doForwardKinematics
(
&
m_kin_model
);
doForwardKinematics
();
std
::
cout
<<
"pose "
<<
m_position
<<
"
\n
"
<<
m_orientation
<<
std
::
endl
;
}
...
...
@@ -84,7 +84,7 @@ void Yumi::compTaskSpaceInput(){
void
Yumi
::
process
(){
doForwardKinematics
(
&
m_kin_model
);
doForwardKinematics
();
compTaskSpaceInput
();
Eigen
::
Matrix
<
double
,
7
,
1
>
jointVelocities
;
...
...
@@ -102,7 +102,7 @@ Eigen::Matrix<double, 7, 1> Yumi::get_newJointValues(){
}
Eigen
::
Matrix
<
double
,
6
,
1
>
Yumi
::
get_newPose
(){
doForwardKinematics
(
&
m_kin_model
);
doForwardKinematics
();
Eigen
::
Matrix
<
double
,
6
,
1
>
pose
;
pose
<<
m_position
,
m_orientation
;
return
pose
;
...
...
This diff is collapsed.
Click to expand it.
python/taskspace_placement/computeJoints.py
+
2
−
2
View file @
cb132a06
...
...
@@ -64,7 +64,7 @@ error_right = np.zeros((len(p1[:,0]),6))
jointAngles
=
np
.
array
([
-
110.0
,
29.85
,
35.92
,
49.91
,
117.0
,
123.0
,
-
117.0
])
*
np
.
pi
/
180.0
# loop for the right arm
"""
for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)): # loop through all the desired position of left arm
for
index
,
(
pos
,
vel
,
phi
,
phi_dot
)
in
enumerate
(
zip
(
p2
,
v2
,
phi_delta
,
dphi
)):
# loop through all the desired position of left arm
desPose
=
np
.
concatenate
((
pos
,
phi
),
axis
=
0
)
desVelocities
=
np
.
concatenate
((
vel
,
phi_dot
),
axis
=
0
)
yumi_right
.
set_jointValues
(
jointAngles
,
jointVelocities
)
...
...
@@ -77,7 +77,7 @@ jointAngles = np.array([-110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0]) * np
if
index
>
0
:
jointVelocities
=
(
compJointAngles_left
[
index
,
:]
-
compJointAngles_left
[
index
-
1
,
:])
/
dt
error_right
[
index
,
:]
=
desPose
-
computedPose_right
[
index
,
:]
jointAngles = compJointAngles_right[index,:]
"""
jointAngles
=
compJointAngles_right
[
index
,:]
# see development of joint values
fig
=
plt
.
figure
()
...
...
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