Skip to content

added joint position support and increased planning time #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
MaxDennerle wants to merge 1 commit into
base: hydro
Choose a base branch
from
Open

added joint position support and increased planning time #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions force_torque_sensor_calib/config/example_ft_calib_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,9 @@ random_poses: false
# number of random poses
number_random_poses: 0

# use joint positions instead of cartesian poses
use_joints: false

# the poses to which to move the arm in order to calibrate the F/T sensor
# format: [x y z r p y] in meters, radians
# poses_frame_id sets the frame at which the poses are expressed
Expand Down
51 changes: 39 additions & 12 deletions force_torque_sensor_calib/src/ft_calib_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
#include <moveit/move_group_interface/move_group.h>
#include <force_torque_sensor_calib/ft_calib.h>
#include <eigen3/Eigen/Core>
#include <vector>

using namespace Calibration;

Expand Down Expand Up @@ -179,6 +180,9 @@ class FTCalibNode
// number of random poses
n_.param("number_random_poses", m_number_random_poses, 30);

// poses or joints
n_.param("use_joints", m_use_joints, false);


// initialize the file with gravity and F/T measurements

Expand Down Expand Up @@ -239,22 +243,42 @@ class FTCalibNode
return true;
}

geometry_msgs::Pose pose_;
pose_.position.x = pose(0);
pose_.position.y = pose(1);
pose_.position.z = pose(2);
if(!m_use_joints)
{
ROS_INFO("Using poses");
geometry_msgs::Pose pose_;
pose_.position.x = pose(0);
pose_.position.y = pose(1);
pose_.position.z = pose(2);

tf::Quaternion q;
q.setRPY((double)pose(3), (double)pose(4), (double)pose(5));

tf::Quaternion q;
q.setRPY((double)pose(3), (double)pose(4), (double)pose(5));
tf::quaternionTFToMsg(q, pose_.orientation);

tf::quaternionTFToMsg(q, pose_.orientation);
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.pose = pose_;
pose_stamped.header.frame_id = m_poses_frame_id;
pose_stamped.header.stamp = ros::Time::now();

geometry_msgs::PoseStamped pose_stamped;
pose_stamped.pose = pose_;
pose_stamped.header.frame_id = m_poses_frame_id;
pose_stamped.header.stamp = ros::Time::now();
m_group->setPoseTarget(pose_stamped);
}

m_group->setPoseTarget(pose_stamped);
else
{
ROS_INFO("Using joints");
std::vector<double> group_variable_values(6);


group_variable_values[0] = (double)pose(0);
group_variable_values[1] = (double)pose(1);
group_variable_values[2] = (double)pose(2);
group_variable_values[3] = (double)pose(3);
group_variable_values[4] = (double)pose(4);
group_variable_values[5] = (double)pose(5);

m_group->setJointValueTarget(group_variable_values);
}

}
else // or execute random poses
Expand All @@ -275,6 +299,7 @@ class FTCalibNode


m_pose_counter++;
m_group->setPlanningTime(10.0);
m_group->move();
ROS_INFO("Finished executing pose %d", m_pose_counter-1);
return true;
Expand Down Expand Up @@ -550,6 +575,8 @@ class FTCalibNode
// default: 30
int m_number_random_poses;

bool m_use_joints;

};

int main(int argc, char **argv)
Expand Down