Skip to content
Snippets Groups Projects
Commit cb132a06 authored by Joschua Gosda's avatar Joschua Gosda
Browse files

cleared all pointers from function arguments

parent 66780283
Branches
No related tags found
1 merge request!2Cleanup
......@@ -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();
......
#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;
}
......@@ -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;
......
......@@ -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()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment