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
4fa169a8
Commit
4fa169a8
authored
3 years ago
by
Joschua Gosda
Browse files
Options
Downloads
Patches
Plain Diff
IK works OMG
parent
fcac1a32
No related branches found
No related tags found
1 merge request
!1
all code snippets work
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
c++/gpm.cpp
+7
-4
7 additions, 4 deletions
c++/gpm.cpp
python/taskspace_placement/computeJoints_left.py
+10
-2
10 additions, 2 deletions
python/taskspace_placement/computeJoints_left.py
with
17 additions
and
6 deletions
c++/gpm.cpp
+
7
−
4
View file @
4fa169a8
...
@@ -113,6 +113,8 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
...
@@ -113,6 +113,8 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
effectiveTaskSpaceInput
.
head
(
3
)
=
gainDriftCompensation
/
dt
*
(
desiredTranslation
-
currentTranslation
)
effectiveTaskSpaceInput
.
head
(
3
)
=
gainDriftCompensation
/
dt
*
(
desiredTranslation
-
currentTranslation
)
+
desVelocity
.
head
(
3
);
+
desVelocity
.
head
(
3
);
effectiveTaskSpaceInput
.
tail
(
3
)
=
gainDriftCompensation
/
dt
*
errorRotationInWorldFrame
+
desVelocity
.
tail
(
3
);
effectiveTaskSpaceInput
.
tail
(
3
)
=
gainDriftCompensation
/
dt
*
errorRotationInWorldFrame
+
desVelocity
.
tail
(
3
);
std
::
cout
<<
"effectiveTaskSpaceInput: "
<<
effectiveTaskSpaceInput
<<
std
::
endl
;
// COMPUTE GRADIENTS
// COMPUTE GRADIENTS
// define min and max values for the joints of Yumi
// define min and max values for the joints of Yumi
...
@@ -172,12 +174,13 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
...
@@ -172,12 +174,13 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
Eigen
::
Matrix
<
double
,
7
,
7
>
m_inverseWeighing
=
Eigen
::
Matrix
<
double
,
7
,
7
>
::
Identity
();
Eigen
::
Matrix
<
double
,
7
,
7
>
m_inverseWeighing
=
Eigen
::
Matrix
<
double
,
7
,
7
>
::
Identity
();
double
m_activationFactorTaskSpace
=
1.0
;
double
m_activationFactorTaskSpace
=
1.0
;
Eigen
::
Matrix
<
double
,
7
,
1
>
nullSpaceVelocity
=
-
m_inverseWeighing
*
nullSpaceGradient
;
Eigen
::
Matrix
<
double
,
7
,
1
>
nullSpaceVelocity
=
-
m_inverseWeighing
*
nullSpaceGradient
;
broccoli
::
core
::
math
::
solvePseudoInverseEquation
(
J
,
m_inverseWeighing
,
effectiveTaskSpaceInput
,
nullSpaceVelocity
,
m_activationFactorTaskSpace
);
Eigen
::
Matrix
<
double
,
7
,
1
>
jointVelocities
;
jointVelocities
=
broccoli
::
core
::
math
::
solvePseudoInverseEquation
(
J
,
m_inverseWeighing
,
effectiveTaskSpaceInput
,
nullSpaceVelocity
,
m_activationFactorTaskSpace
);
std
::
cout
<<
"resulting jointVelocities:
\n
"
<<
jointVelocities
<<
std
::
endl
;
// perform integration over one timestep to obtain positions that can be send to robot
// perform integration over one timestep to obtain positions that can be send to robot
jointAnglesDelta
<<
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
;
jointAnglesDelta
<<
jointVelocities
*
dt
;
// integrate value over one time step
jointAnglesDelta
*=
dt
;
//std::cout << "current qs in DEG \n" << jointAngles* rl::math::RAD2DEG << std::endl;
//std::cout << "current qs in DEG \n" << jointAngles* rl::math::RAD2DEG << std::endl;
std
::
cout
<<
"delta qs in DEG
\n
"
<<
jointAnglesDelta
*
rl
::
math
::
RAD2DEG
<<
std
::
endl
;
std
::
cout
<<
"delta qs in DEG
\n
"
<<
jointAnglesDelta
*
rl
::
math
::
RAD2DEG
<<
std
::
endl
;
...
...
This diff is collapsed.
Click to expand it.
python/taskspace_placement/computeJoints_left.py
+
10
−
2
View file @
4fa169a8
...
@@ -64,8 +64,8 @@ computedPose = np.zeros((len(p1[:,0]),6))
...
@@ -64,8 +64,8 @@ computedPose = np.zeros((len(p1[:,0]),6))
error
=
np
.
zeros
((
len
(
p1
[:,
0
]),
6
))
error
=
np
.
zeros
((
len
(
p1
[:,
0
]),
6
))
for
index
,
(
pos
,
vel
,
phi
,
phi_dot
)
in
enumerate
(
zip
(
p1
,
v1
,
phi_total
,
dphi
)):
# loop through all the desired position of left arm
for
index
,
(
pos
,
vel
,
phi
,
phi_dot
)
in
enumerate
(
zip
(
p1
,
v1
,
phi_total
,
dphi
)):
# loop through all the desired position of left arm
desPose
=
np
.
concatenate
((
pos
,
phi
_const
),
axis
=
0
)
# note that phi_const is used -> same orientation throughout the movement
desPose
=
np
.
concatenate
((
pos
,
phi
),
axis
=
0
)
# note that phi_const is used -> same orientation throughout the movement
desVelocities
=
np
.
concatenate
((
vel
,
d
phi_
cons
t
),
axis
=
0
)
# same here
desVelocities
=
np
.
concatenate
((
vel
,
phi_
do
t
),
axis
=
0
)
# same here
# call the c++ egm function, return joint values and resulting pose
# call the c++ egm function, return joint values and resulting pose
result
=
invKin
.
gpm
(
desPose
,
desVelocities
,
jointAngles
,
jointVelocities
,
1
)
result
=
invKin
.
gpm
(
desPose
,
desVelocities
,
jointAngles
,
jointVelocities
,
1
)
desJointAngles
[
index
,:]
=
result
[
0
]
# computed joint values from IK
desJointAngles
[
index
,:]
=
result
[
0
]
# computed joint values from IK
...
@@ -113,6 +113,14 @@ plt.legend()
...
@@ -113,6 +113,14 @@ plt.legend()
plt
.
title
(
'
view on the x-z plane from the right arm side of yumi
'
)
plt
.
title
(
'
view on the x-z plane from the right arm side of yumi
'
)
plt
.
show
()
plt
.
show
()
fig
=
plt
.
figure
()
plt
.
plot
(
computedPose
[:,
3
],
label
=
'
rx
'
)
plt
.
plot
(
computedPose
[:,
4
],
label
=
'
ry
'
)
plt
.
plot
(
computedPose
[:,
5
],
label
=
'
rz
'
)
plt
.
legend
()
plt
.
title
(
'
euler angles over trajectories
'
)
plt
.
show
()
np
.
save
(
'
desJointAngles_left
'
,
desJointAngles
)
np
.
save
(
'
desJointAngles_left
'
,
desJointAngles
)
...
...
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