sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-13 21:48:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'GEAR_arm_controller' of github.com:EECS-467-W20-RRRobot-Project/RRRobot into GEAR_arm_controller

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-26 22:35:37 -04:00
parent 74d9d40a7b b761daf3b3
commit 2b8e29c189
11 changed files with 119 additions and 213 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -34,3 +34,4 @@ core
*.old
*.custom
*.run
*.debug

32
docs/gear.md
View File

@@ -86,18 +86,38 @@ rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory "{joint_
## Running Full Simulation
### ARIAC Environment
### Terminal 1: ARIAC Environment
- `cd /app/rrrobot_ws/src/rrrobot/scripts`
- `./rrrobot_run.sh`
- `./rrrobot_run_no_build.sh`
### Build RRRobot Package
### Terminal 2: Build RRRobot Package & Arm Controller Node
- `cd /app/rrrobot_ws`
- `catkin_make clean`
- `catkin_make`
- `source devel/setup.bash`
### Arm Controller Node
- `rosrun rrrobot arm_controller_node`
### Terminal 3: CV Model
- `cd /app/rrrobot_ws/src/rrrobot/src/`
- `python3 cv_model.py`
### Terminal 4: Depth Camera Node (for getting pick up location)
- `cd /app/rrrobot_ws/`
- `source devel/setup.bash`
- `rosrun rrrobot depth_camera_node`
### Terminal 5: Run the main rrrobot node
- `cd /app/rrrobot_ws/`
- `source devel/setup.bash`
- `rosrun rrrobot rrrobot_node`
### Terminal 6: Run the node to spawn random objects onto the conveyor belt
- `cd /app/rrrobot_ws/`
- `source devel/setup.bash`
- `rosrun rrrobot object_spawner_node`

2
src/rrrobot_ws/src/rrrobot/include/arm_representation.h
View File

@@ -43,7 +43,7 @@ public:
int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr = -1);
int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
int calculateInverseKinematics(const std::vector<double> &cur_configuration,
const KDL::Frame &desired_end_effector_pose,
KDL::JntArray &final_joint_configuration);

17
src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh
View File

@@ -1,11 +1,12 @@
#!/bin/bash
#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}'
source /app/rrrobot_ws/devel/setup.bash
rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
position:
x: 1.2
y: -1
z: 1.5
x: 1.22
y: 1.22
z: 0.9725
orientation:
x: 0.0
y: 0.707
@@ -14,11 +15,11 @@ rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
drop_location:
position:
x: -0.3
y: 1.15
z: 1.5
y: 0.383
z: 1.0
orientation:
x: 0.0
y: 0.0
y: 0.707
z: 0.0
w: 0.0" \
w: 0.707" \
-1

110
src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
View File

@@ -48,10 +48,12 @@ public:
gripper_ = node.serviceClient<osrf_gear::VacuumGripperControl>(GRIPPER_CONTROL_CHANNEL);
arm_current_joint_states_.resize(arm->getChain()->getNrOfJoints(), 0.0);
// arm = std::move(arm_);
// ROS_INFO("Arm size (in constructor): %d", arm->getChain()->getNrOfJoints());
arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
KDL::SetToZero(arm_current_joint_states_);
// arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
// KDL::SetToZero(arm_current_joint_states_);
}
void joint_state_callback(const sensor_msgs::JointState &joint_state_msg)
@@ -64,22 +66,9 @@ public:
vector<string> cur_joint_names = arm->get_joint_names();
vector<double> position = joint_state_msg.position;
// Print joint name vectors for debugging
// ROS_INFO_STREAM("msg_joint_names: {");
// for (int i = 0; i < nbr_joints; ++i)
// {
// ROS_INFO_STREAM(msg_joint_names[i] << ", ");
// }
// ROS_INFO_STREAM("}\ncur_joint_names: {");
// for (int j = 0; j < cur_joint_names.size(); ++j)
// {
// ROS_INFO_STREAM(cur_joint_names[j] << ", ");
// }
// ROS_INFO_STREAM("}");
for (int i = 0; i < nbr_joints; ++i)
{
arm_current_joint_states_(i) = 0.0;
arm_current_joint_states_[i] = 0.0;
}
for (size_t i = 0; i < position.size(); ++i)
@@ -88,7 +77,7 @@ public:
{
if (msg_joint_names[i].compare(cur_joint_names[j]) == 0)
{
arm_current_joint_states_(j) = position[i];
arm_current_joint_states_[j] = position[i];
}
}
}
@@ -122,7 +111,7 @@ public:
// Move arm to pickup item from conveyor belt
attempts = 1;
positions = calc_joint_positions(target_pose.grab_location);
positions = calc_joint_positions(target_pose.grab_location, above_conveyor);
send_joint_trajectory(positions, arm_action_phase::bin_to_belt);
// Check if target has been reached
@@ -151,7 +140,7 @@ public:
// If item is not able to attach, adjust z and try again
target_location.position.z -= item_attach_z_adjustment; // meters
positions = calc_joint_positions(target_location);
positions = calc_joint_positions(target_location, arm_current_joint_states_);
send_joint_trajectory(positions, arm_action_phase::item_pickup_adjustment);
attempts++;
}
@@ -160,11 +149,11 @@ public:
// Move item to desired position
attempts = 1;
positions = calc_joint_positions(target_pose.drop_location);
positions = calc_joint_positions(target_pose.drop_location, above_bins);
send_joint_trajectory(positions, arm_action_phase::belt_to_bin);
// Check if target has been reached
while(!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location)) {
while(!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location)) {
if (attempts == retry_attempts) {
ROS_ERROR("STEP 4: Did not reach target in allotted time");
return;
@@ -198,7 +187,7 @@ public:
ROS_ERROR("STEP 6: Item not detaching.. ¯\\_(ツ)_/¯");
}
ros::Duration(0.1).sleep();
ros::Duration(1).sleep();
attempts++;
}
@@ -206,11 +195,12 @@ public:
}
private:
// Private variables
bool gripper_enabled_;
bool item_attached_;
ros::Publisher arm_joint_trajectory_publisher_;
ros::ServiceClient gripper_;
KDL::JntArray arm_current_joint_states_;
vector<double> arm_current_joint_states_;
ArmRepresentation *arm;
enum arm_action_phase {
belt_to_bin,
@@ -220,8 +210,11 @@ private:
double time_per_step;
int retry_attempts;
double item_attach_z_adjustment;
// Define intermediate joint positions
const vector<double> above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
const vector<double> above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
vector<double> calc_joint_positions(const geometry_msgs::Pose &pose)
vector<double> calc_joint_positions(const geometry_msgs::Pose &pose, const vector<double> &start_state)
{
// KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_;
// KDL::SetToZero(cur_configuration);
@@ -230,7 +223,7 @@ private:
// ROS_INFO("cur_configuration (%i x %i)", cur_configuration.rows(), cur_configuration.columns());
int error_code = arm->calculateInverseKinematics(arm_current_joint_states_, desired_end_effector_pose, final_joint_configuration);
int error_code = arm->calculateInverseKinematics(start_state, desired_end_effector_pose, final_joint_configuration);
// Check status of IK and print error message if there is a failure
if (error_code != 0)
@@ -253,13 +246,16 @@ private:
KDL::Frame calc_end_effector_pose()
{
int num_joints = arm->getChain()->getNrOfJoints();
KDL::Frame end_effector_pose;
// KDL::JntArray joint_positions = arm_current_joint_states_;
KDL::JntArray joint_positions(arm->getChain()->getNrOfJoints());
KDL::SetToZero(joint_positions);
KDL::JntArray joint_positions(num_joints);
// Copy current joint states values into JntArray
for (int i = 0; i < num_joints; ++i) {
joint_positions(i) = arm_current_joint_states_[i];
}
// ROS_INFO("joint_positions (%i x %i)", joint_positions.rows(), joint_positions.columns());
// ROS_INFO("arm chain # of joints: %d", arm->getChain()->getNrOfJoints());
int error_code = arm->calculateForwardKinematics(joint_positions, end_effector_pose);
// Check status of FK and do something if there is a failure
@@ -315,10 +311,6 @@ private:
// Declare JointTrajectory message
trajectory_msgs::JointTrajectory msg;
// Define intermediate joint positions
const vector<double> above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
const vector<double> above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
// Fill the names of the joints to be controlled
msg.joint_names = arm->get_joint_names();
@@ -383,51 +375,52 @@ private:
bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
{
// TODO: Tune threshold values
float pos_thresh = 0.1; // Meters
float rot_thresh = 0.0875; // Radians
// Tune threshold values
float pos_thresh = 0.15; // Meters
float rot_thresh = 0.05;
float pos_err = fabs(cur.position.x - target.position.x) +
fabs(cur.position.y - target.position.y) +
fabs(cur.position.z - target.position.z);
// ROS_INFO_STREAM("pos_err: " << pos_err << " (" << pos_thresh << ")");
if (pos_err > pos_thresh)
{
return false;
}
vector<double> rpy1 = QuatToEuler(cur.orientation);
vector<double> rpy2 = QuatToEuler(target.orientation);
float qx_err = fabs(fabs(cur.orientation.x) - fabs(target.orientation.x));
float qy_err = fabs(fabs(cur.orientation.y) - fabs(target.orientation.y));
float qz_err = fabs(fabs(cur.orientation.z) - fabs(target.orientation.z));
float qw_err = fabs(fabs(cur.orientation.w) - fabs(target.orientation.w));
float rx_err = fabs(fmod(rpy1[0] - rpy2[0] + M_PI, M_PI));
float ry_err = fabs(fmod(rpy1[1] - rpy2[1] + M_PI, M_PI));
float rz_err = fabs(fmod(rpy1[2] - rpy2[2] + M_PI, M_PI));
ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")");
ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")");
ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")");
ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")");
if (rx_err > rot_thresh)
if (qx_err > rot_thresh)
{
return false;
}
if (ry_err > rot_thresh)
if (qy_err > rot_thresh)
{
return false;
}
if (rz_err > rot_thresh)
if (qz_err > rot_thresh)
{
return false;
}
if (qw_err > rot_thresh) {
return false;
}
return true;
}
vector<double> QuatToEuler(geometry_msgs::Quaternion q) {
KDL::Rotation r = KDL::Rotation::Quaternion(q.x, q.y, q.z, q.w);
vector<double> rpy = {0, 0, 0};
r.GetRPY(rpy[0], rpy[1], rpy[2]);
return rpy;
}
};
int main(int argc, char **argv)
@@ -443,20 +436,23 @@ int main(int argc, char **argv)
double time_per_step = 3.0; // seconds
int retry_attempts = 3;
double item_attach_z_adjustment = 0.025; // meters
double item_attach_z_adjustment = 0.05; // meters
ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment);
ros::AsyncSpinner spinner(0);
spinner.start();
// Subscribe to arm destination and joint states channels
ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac);
ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 10, &ArmController::gripper_state_callback, &ac);
ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 1, &ArmController::gripper_state_callback, &ac);
ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 10, &ArmController::joint_state_callback, &ac);
ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 1, &ArmController::joint_state_callback, &ac);
ROS_INFO("Setup complete");
// Execute callbacks on new data until ctrl-c
ros::spin();
ros::waitForShutdown();
return 0;
}

14
src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp
View File

@@ -99,12 +99,22 @@ int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_pos
return status;
}
int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_configuration,
int ArmRepresentation::calculateInverseKinematics(const vector<double> &cur_configuration,
const KDL::Frame &desired_end_effector_pose,
KDL::JntArray &final_joint_configuration)
{
KDL::ChainIkSolverPos_LMA ik_solver(chain, 1e-3, 2000, 1e-8);
int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
// Convert cur_configuration vector to JntArray
int num_joints = chain.getNrOfJoints();
KDL::JntArray pos(num_joints);
KDL::SetToZero(pos);
for (int i = 0; i < num_joints; ++i) {
pos(i) = cur_configuration[i];
}
int status = ik_solver.CartToJnt(pos, desired_end_effector_pose, final_joint_configuration);
return status;
}

2
src/rrrobot_ws/src/rrrobot/src/cv_model.py
View File

@@ -38,7 +38,7 @@ def call_back(filename):
# query model
predicted_logits = model(input_tensor).squeeze()
# print(predicted_logits.shape)
predicted_label = torch.argmax(predicted_logits).numpy().tolist()
predicted_label = torch.argmax(predicted_logits).detach().numpy().tolist()
type_dict = {0: 'cardboard',
1: 'glass' ,

22
src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp
View File

@@ -218,7 +218,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
geometry_msgs::Point p;
geometry_msgs::Quaternion q;
// pick up from top
// pick up from top (-z direction)
if (area_top > .002)
{
std::cout << "picking up object from top" << std::endl;
@@ -228,23 +228,23 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
// rpy = 0 0 0
q.x = 0;
q.y = 0;
q.y = 0.707;
q.z = 0;
q.w = 1;
q.w = 0.707;
}
else
{
// pick up from front
// pick up from front (+x direction)
std::cout << "picking up object from front" << std::endl;
p.x = xmin;
p.y = (ymin + ymax) / 2;
p.z = (zmax + zmin) / 2;
// rpy = -pi/2 0 pi/2
q.x = -0.63;
q.y = 0;
q.z = 0.63;
q.w = 0.44;
q.x = 0.707;
q.y = 0.0;
q.z = 0.0;
q.w = 0.707;
}
pose.position = p;
@@ -271,18 +271,16 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
int main(int argc, char **argv)
{
// TODO: Subscribe to depth camera topic
// Last argument is the default name of the node.
ros::init(argc, argv, "depth_camera_node");
ros::NodeHandle node;
// Subscribe to depth camera topic
ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
// Publish object's current location to topic for RRRobot node to listen to
pub = node.advertise<geometry_msgs::Pose>(DESIRED_GRASP_POSE_CHANNEL, 1);
ros::spin();
// TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item
// TODO: Publish object's current location to topic for RRRobot node to listen to
}

2
src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp
View File

@@ -99,7 +99,7 @@ private:
image_file_msg.data = models[rand_idx].image_file;
cout << "Spawning " << msg.model_name << endl;
// TODO: Determine position to spawn items
// Determine position to spawn items
// can place objects on conveyor belt just upstream from the arms
// at roughly (0.2516105, 5.474367, 0.935669)
// x range: 1.022548-1.413952

2
src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp
View File

@@ -126,6 +126,8 @@ public:
set_conveyor(0);
desired_grasp_pose = grasp_pose;
// TODO: Tune z offset so end effector doesn't hit object
desired_grasp_pose.position.z -= 0.025;
if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
{

128
src/rrrobot_ws/src/rrrobot/test/test_arm.cpp
View File

@@ -36,130 +36,8 @@
#include "arm_representation.h"
// using namespace std;
using std::cin;
using std::cout;
using std::endl;
using std::string;
using namespace std;
// class ArmRepresentation
// {
// public:
// ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1)))
// {
// const double base_len = 0.2;
// const double shoulder_height = 0.1273;
// const double upper_arm_length = 0.612;
// const double forearm_length = 0.5723;
// const double shoulder_offset = 0.220941;
// const double elbow_offset = -0.1719;
// const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset;
// const double wrist_2_length = 0.1157;
// const double wrist_3_length = 0.0922;
// // KDL::Vector pos; //(0, 0, base_len/2);
// // KDL::Rotation rot;
// // The joints might be off by one
// KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
// KDL::Joint(KDL::Joint::JointType::None), base_pose);
// arm.addSegment(linear_arm_actuator);
// const KDL::Vector pos_base(0, 0, base_len / 2);
// const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0));
// KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::TransY),
// KDL::Frame(rot_base, pos_base));
// arm.addSegment(base_link);
// const KDL::Vector pos_shoulder(0, 0, shoulder_height);
// const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0));
// KDL::Segment shoulder_link("shoulder_link", KDL::Joint(KDL::Joint::JointType::RotZ),
// KDL::Frame(rot_shoulder, pos_shoulder));
// arm.addSegment(shoulder_link);
// const KDL::Vector pos_upper_arm(0, shoulder_offset, 0);
// const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
// KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_upper_arm, pos_upper_arm));
// arm.addSegment(upper_arm_link);
// const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length);
// const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment forearm_link("forearm_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_forearm, pos_forearm));
// arm.addSegment(forearm_link);
// const KDL::Vector pos_wrist_1(0, 0, forearm_length);
// const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
// KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_wrist_1, pos_wrist_1));
// arm.addSegment(wrist_1_link);
// const KDL::Vector pos_wrist_2(0, wrist_1_length, 0);
// const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint(KDL::Joint::JointType::RotZ),
// KDL::Frame(rot_wrist_2, pos_wrist_2));
// arm.addSegment(wrist_2_link);
// const KDL::Vector pos_wrist_3(0, 0, wrist_2_length);
// const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_wrist_3, pos_wrist_3));
// arm.addSegment(wrist_3_link);
// const KDL::Vector pos_ee(0, wrist_3_length, 0.0);
// const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0));
// KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
// KDL::Frame(rot_ee, pos_ee));
// arm.addSegment(ee_link);
// // arm.addSegment(base_link);
// // arm.addSegment(shoulder_link);
// // arm.addSegment(upper_arm_link);
// // arm.addSegment(forearm_link);
// // arm.addSegment(wrist_1_link);
// // arm.addSegment(wrist_2_link);
// // arm.addSegment(wrist_3_link);
// // arm.addSegment(ee_link);
// fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
// ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
// }
// int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
// {
// return fk_solver->JntToCart(joint_positions, end_effector_pose);
// }
// int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
// const KDL::Frame &desired_end_effector_pose,
// KDL::JntArray &final_joint_configuration)
// {
// return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
// }
// string *get_joint_names()
// {
// // TODO: return ["linear_arm_actuator_joint",
// // "shoulder_pan_joint",
// // "shoulder_lift_joint",
// // "elbow_joint",
// // "wrist_1_joint",
// // "wrist_2_joint",
// // "wrist_3_joint"];
// }
// const KDL::Chain &getArm() const
// {
// return arm;
// }
// private:
// KDL::Chain arm;
// std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
// std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
// };
int main(int argc, char **argv)
{
@@ -174,7 +52,7 @@ int main(int argc, char **argv)
// std::cin >> joint;
KDL::Frame end_effector_pose;
std::ofstream f("data.txt");
std::ofstream f("arm_test.debug");
int error_code;
for (joint = 0; joint < arm.getChain()->getNrOfJoints(); ++joint)
{
@@ -190,7 +68,7 @@ int main(int argc, char **argv)
f << end_effector_pose.p.x() << "," << end_effector_pose.p.y() << "," << end_effector_pose.p.z() << '\n';
error_code = arm.calculateInverseKinematics(pos, end_effector_pose, pos);
error_code = arm.calculateInverseKinematics(vector<double>(arm.getChain()->getNrOfJoints(), 0.0), end_effector_pose, pos);
if (error_code != 0)
{