flyingRobot78's profile - activity

I am trying to plan two subsequent motions with Moveit. The first motion is a relatively unconstrained motion to an end effector pose. I would like the second motion to move the first roll joint to a specified position while maintaining the end effector pose. (self motion.)

I can plan the first motion just fine. I am trying to acheive the self motion by adding a goal constraint to the original motion plan request, and adding a path constraint that fixes the end effector pose.

Relevant code:

 //First motion:
 moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("link_t", pose, tolerance_pose, tolerance_angle);  
  mp_request.goal_constraints.push_back(pose_goal);
  mp_request.allowed_planning_time = 10.0;

  moveit_msgs::GetMotionPlan mp_srv;
  mp_srv.request.motion_plan_request = mp_request;
  mp_srv.response.motion_plan_response = mp_response;
  if(!mp_client.call(mp_srv)){
    std::cout << "Motion planning failure: error_code: " << mp_srv.response.motion_plan_response.error_code.val << std::endl; 
  }
  mp_response = mp_srv.response.motion_plan_response;
  std::cout << "trajectory size: " << mp_response.trajectory.joint_trajectory.points.size() << std::endl;

  kinematic_state->setStateValues(mp_response.trajectory.joint_trajectory.points.back().positions);
  joint_state_msg.name = joint_state_group->getJointModelGroup()->getJointModelNames();
  joint_state_msg.position = mp_response.trajectory.joint_trajectory.points.back().positions;
  joint_state_publisher.publish(joint_state_msg);

  ros::spinOnce();

  current_joints_ = joint_state_msg;
  mp_request.start_state.joint_state = current_joints_;

  ////////Self motion/////////////////////////
  moveit_msgs::Constraints path_eef_constraints_, joint_constraints_;
  moveit_msgs::JointConstraint goal_joint_constraint_;
  moveit_msgs::OrientationConstraint eef_orientation_constraint_;
  moveit_msgs::PositionConstraint eef_pos_constraint_;

  //Set Joint_1 goal constraint and add to goal_constraints.
  goal_joint_constraint_.joint_name = joint_state_msg.name.at(0);
  goal_joint_constraint_.position = current_joints_.position.at(0)-1.57;
  goal_joint_constraint_.position = 0;
  goal_joint_constraint_.tolerance_above = .1;
  goal_joint_constraint_.tolerance_below = .1;
  goal_joint_constraint_.weight = .5;

  //add joint goal constraint:    
  mp_request.goal_constraints.back().joint_constraints.push_back(goal_joint_constraint_);

  //Set path constraint:
  mp_request.path_constraints = pose_goal;
  mp_srv.request.motion_plan_request = mp_request;

The first motion goes off without a hitch, and If I do the second motion with the joint specified, but no path constraint, it plans correctly as well. However, when I add the path constraint, I get the following errors:

[ERROR] [1368026713.167017264]: Poses vector must have size: 1
[ERROR] [1368026713.167240823]: Poses vector must have size: 1
[ INFO] [1368026713.168012375]: No planner specified. Using default.
[ WARN] [1368026713.168312286]: Skipping invalid start state (invalid bounds)
[ERROR] [1368026713.168460131]: Motion planning start tree could not be initialized!
[ INFO] [1368026713.168488571]: No solution found after 0.000251 seconds
[ WARN] [1368026713.177067471]: Goal sampling thread never did any work.
[ INFO] [1368026713.177400962]: Unable to solve the planning problem"
Motion planning failure: error_code: 1

I would guess that the Poses vector error is what is killing me, but I'm not sure what's triggering this error. Can anyone provide any insight?

Hello,

The database_interface package appears to provide serialization for storing data in the database as a byte array. The advanced concepts tutorial shows an example of how to read from the binary data, but not how to write to the database. Let's say I want to store a sensor_msgs::PointCloud2 or a pcl::Pointcloud to the database. How would I read/write it?

I currently have a database set up in postgres that's ready to accept the binary info as bytea data. In the database interface classes, I have:

database_interface::DBField< std::vector< char > > viewpoint_1_;

and in the constructor I have:

viewpoint_1_(database_interface::DBFieldBase::BINARY, this, "viewpoint_1", "viewpoint", true)

I think this is set up properly, but I don't know how to get the data in and out of the database. Can anyone point me in the right direction?