//-----------------------------------------------------------------------------
// clampRotation()
//-----------------------------------------------------------------------------
void LLJoint::clampRotation(LLQuaternion old_rot, LLQuaternion new_rot)
{
LLVector3 main_axis(1.f, 0.f, 0.f);
for (child_list_t::iterator iter = mChildren.begin();
iter != mChildren.end(); ++iter)
{
LLJoint* joint = *iter;
if (joint->isAnimatable())
{
main_axis = joint->getPosition();
main_axis.normVec();
// only care about first animatable child
break;
}
}
// 2003.03.26 - This code was just using up cpu cycles. AB
// LLVector3 old_axis = main_axis * old_rot;
// LLVector3 new_axis = main_axis * new_rot;
// for (S32 i = 0; i < mConstraintSilhouette.count() - 1; i++)
// {
// LLVector3 vert1 = mConstraintSilhouette[i];
// LLVector3 vert2 = mConstraintSilhouette[i + 1];
// figure out how to clamp rotation to line on 3-sphere
// }
}
//-----------------------------------------------------------------------------
// clampRotation()
//-----------------------------------------------------------------------------
void LLJoint::clampRotation(LLQuaternion old_rot, LLQuaternion new_rot)
{
LLVector3 main_axis(1.f, 0.f, 0.f);
for (child_list_t::iterator iter = mChildren.begin();
iter != mChildren.end(); ++iter)
{
LLJoint* joint = *iter;
if (joint->isAnimatable())
{
main_axis = joint->getPosition();
main_axis.normVec();
// only care about first animatable child
break;
}
}
}
int main(int argc, char **argv){
ros::init (argc, argv, "circ_trajectory_viz_execution");
ros::NodeHandle n;
ros::AsyncSpinner spinner(1);
spinner.start();
ros::Publisher rvizMarkerPub;
rvizMarkerPub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1000);
moveit::planning_interface::MoveGroup group("right_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// (Optional) Create a publisher for visualizing plans in Rviz.
//ros::Publisher display_publisher = n.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
//moveit_msgs::DisplayTrajectory display_trajectory;
// Getting Basic Information
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//
// We can print the name of the reference frame for this robot.
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
// Move the robot to a known starting position
robot_state::RobotState start_state(*group.getCurrentState());
geometry_msgs::Pose start_pose2;
start_pose2.orientation.x = 1.0;
start_pose2.orientation.y = 0.0;
start_pose2.orientation.z = 0.0;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;//0.55;
start_pose2.position.y = -0.55;//-0.05;
start_pose2.position.z = 0.8;//0.8;
const robot_state::JointModelGroup *joint_model_group =
start_state.getJointModelGroup(group.getName());
start_state.setFromIK(joint_model_group, start_pose2);
group.setStartState(start_state);
// Now we will plan to the earlier pose target from the new
// start state that we have just created.
group.setPoseTarget(start_pose2);
moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
ROS_INFO("Moving to start position %s",success?"":"FAILED");
group.move();
/* Sleep to give Rviz time to visualize the plan. */
sleep(10.0);
// When done with the path constraint be sure to clear it.
group.clearPathConstraints();
ROS_INFO("Opening Bag");
rosbag::Bag bag;
bag.open("/home/stevenjj/catkin_ws/src/hw3/circular_forward_trajectory.bag", rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("visualization_marker")); //Specify topic to read
rosbag::View view(bag, rosbag::TopicQuery(topics));
// Define Reference quaternion to be the x-axis.
//tf::Quaternion main_axis(1,0,0, 1);
tf::Vector3 r_gripper_position(start_pose2.position.x, start_pose2.position.y, start_pose2.position.z);
tf::Quaternion main_axis(start_pose2.orientation.x, start_pose2.orientation.y, start_pose2.orientation.z, start_pose2.orientation.w);
main_axis = main_axis.normalize();
// Define the position and orientation of the first marker
tf::Vector3 first_marker_vector_offset;
tf::Quaternion first_marker_axis;
// Define Quaternion Rotation Offset
tf::Quaternion axis_rotation;
int marker_index = 0;
// Count total number of markers
// Identify the first marker and use its position and orientation to identify the first rotation
int total_markers = 0;
foreach(rosbag::MessageInstance const m, view){
if (total_markers == 0){
visualization_msgs::Marker::ConstPtr first_marker = m.instantiate<visualization_msgs::Marker>();
first_marker_vector_offset.setX(first_marker->pose.position.x);
first_marker_vector_offset.setY(first_marker->pose.position.y);
first_marker_vector_offset.setZ(first_marker->pose.position.z);
tf::Quaternion store_axis (first_marker->pose.orientation.x,
first_marker->pose.orientation.y,
first_marker->pose.orientation.z,
first_marker->pose.orientation.w);
first_marker_axis = store_axis;
}
total_markers++; // count total number of markers in the rosbag
}
// Fancy Quaternion math. This took forever. p' = p_a^-1 * p2 gives the rotation needed to go from p_a to p2.
axis_rotation = first_marker_axis.inverse() * main_axis; // Find axis of rotation
tf::Transform transform_to_main_axis(tf::Quaternion(0,0,0,1), r_gripper_position - first_marker_vector_offset); // Create transform
tf::Transform rotate_to_main_axis(axis_rotation, tf::Vector3(0,0,0)); // Create transform
foreach(rosbag::MessageInstance const m, view){
//std::cout << m.getTopic() << std::endl;
ROS_INFO("Inside bag!");
//geometry_msgs::Pose::ConstPtr p = m.instantiate<geometry_msgs::Pose>();
visualization_msgs::Marker::ConstPtr p = m.instantiate<visualization_msgs::Marker>();
//std::cout << m.getDataType() << std::endl; // Identify topic type
//std::cout << p->pose.position.x << std::endl;
pub_recorded_marker(rvizMarkerPub, p, marker_index, total_markers, transform_to_main_axis, rotate_to_main_axis);
marker_index++;
//sleep(1.0);
std::cout << total_markers << std::endl;
}
int main(int argc, char **argv){
ros::init (argc, argv, "moveit_rosbag");
ros::NodeHandle n;
ros::Publisher rvizMarkerPub;
rvizMarkerPub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
ROS_INFO("Opening Bag");
rosbag::Bag bag;
bag.open("trajectory_bag.bag", rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("visualization_marker")); //Specify topic to read
rosbag::View view(bag, rosbag::TopicQuery(topics));
// Define Reference quaternion to be the x-axis.
tf::Quaternion main_axis(1,0,0, 1);
main_axis = main_axis.normalize();
// Define the position and orientation of the first marker
tf::Vector3 first_marker_vector_offset;
tf::Quaternion first_marker_axis;
// Define Quaternion Rotation Offset
tf::Quaternion axis_rotation;
int marker_index = 0;
// Count total number of markers
// Identify the first marker and use its position and orientation to identify the first rotation
int total_markers = 0;
foreach(rosbag::MessageInstance const m, view){
if (total_markers == 0){
visualization_msgs::Marker::ConstPtr first_marker = m.instantiate<visualization_msgs::Marker>();
first_marker_vector_offset.setX(first_marker->pose.position.x);
first_marker_vector_offset.setY(first_marker->pose.position.y);
first_marker_vector_offset.setZ(first_marker->pose.position.z);
tf::Quaternion store_axis (first_marker->pose.orientation.x,
first_marker->pose.orientation.y,
first_marker->pose.orientation.z,
first_marker->pose.orientation.w);
first_marker_axis = store_axis;
}
total_markers++; // count total number of markers in the rosbag
}
axis_rotation = first_marker_axis.inverse() * main_axis; // Find axis of rotation
tf::Transform transform_to_main_axis(tf::Quaternion(0,0,0,1), -first_marker_vector_offset); // Create transform
tf::Transform rotate_to_main_axis(axis_rotation, tf::Vector3(0,0,0)); // Create transform
//tf::Transform transform_to_main_axis(axis_rotation, -first_marker_vector_offset); // Create transform
// ROS_INFO("non-zero");
// std::cout << first_marker_vector_offset.getZ() << std::endl;
// sleep(1);
foreach(rosbag::MessageInstance const m, view){
//std::cout << m.getTopic() << std::endl;
ROS_INFO("Inside bag!");
//geometry_msgs::Pose::ConstPtr p = m.instantiate<geometry_msgs::Pose>();
visualization_msgs::Marker::ConstPtr p = m.instantiate<visualization_msgs::Marker>();
std::cout << m.getDataType() << std::endl; // Identify topic type
std::cout << p->id << std::endl;
std::cout << p->pose.position.x << std::endl;
std::cout << p->pose.position.y << std::endl;
std::cout << p->pose.position.z << std::endl;
std::cout << p->pose.orientation.x << std::endl;
std::cout << p->pose.orientation.y << std::endl;
std::cout << p->pose.orientation.z << std::endl;
std::cout << p->pose.orientation.w << std::endl;
pub_recorded_marker(rvizMarkerPub, p, marker_index, total_markers, transform_to_main_axis, rotate_to_main_axis);
marker_index++;
//sleep(1.0);
std::cout << total_markers << std::endl;
}
