It is similar to desartes, so tool is unconstrained. I want to constrain fully

[lakshmip001]

I want to constrain fully and achieve a straight line motion. Kindly please let me know how I can fully constrain a straight line path with Cartesian coordinates and Cartesian velocities.

[JeroenDM]

An answer based on what I learned while curiously reading through the code.

Inside descartes_light there are two options to specify a trajectory point (~Cartesian path constraint):

• A fully constraint tool pose CartesianPointSampler.
• Tool pose that is allowed to rotate around the local Z-axis AxialSymmetricSampler. (Rotation around the other axes and tool position fully constrained.)

In addition there are two extra options for robots on a rail: RailedCartesianPointSampler and RailedAxialSymmetricSampler.

The trajopt part of the answer is more a guess for me, but the tool pose constraints are specified in workcell1_demo.cpp like this:

  // Populate Constraints
  for (std::size_t i = 0; i < pass.size(); ++i)
  {
    auto pose = std::make_shared<trajopt::StaticPoseCostInfo>();
    pose->term_type = trajopt::TT_CNT;
    pose->name = "waypoint_cart_" + std::to_string(i);
    pose->link = "sander_tcp";
    pose->timestep = i;
    pose->xyz = pass[i].translation();
    pose->wxyz = to_wxyz(pass[i]);
    pose->pos_coeffs = Eigen::Vector3d(10, 10, 10);
    pose->rot_coeffs = Eigen::Vector3d(10, 10, 0);
    pci.cnt_infos.push_back(pose);
}

Change the coefficient for the orientation constraints from 0

pose->rot_coeffs = Eigen::Vector3d(10, 10, 0);

to 10

pose->rot_coeffs = Eigen::Vector3d(10, 10, 10);

will add a fully constrained tool pose.

See also the basic_cartesian_plan example in the trajopt_ros repository.

[lakshmip001]

Hi,

For attaining constant cartesian velocity i need to provide constraint for CartVelTermInfo for two waypoints (when moving from one point to other point). And hybrid_planning_experiments uses opw_kinematics, while universal robot doesnt have opw_kinematics. I tried trajopt basic_cartesian_plan_example, I was able to visualize the plan in RVIZ, while trying to integrate with gazebo or real robot I am unable to integrate as the trajectory optimized doesn't contain joint velocities and accelerations as shown in the below link.
tesseract-robotics/trajopt#65

Kindly please guide me how to integrate with robot .

[Levi-Armstrong]

Currently you will need to pass the trajectory to time parameterization. I recommend using MoveIts time parameterization libraries.

[lakshmip001]

@Levi-Armstrong hi, I tried calculating velocities through time parameterization by adding following lines:
//robot_trajectory::RobotTrajectory rt(mgi->getCurrentState()->getRobotModel(), "manipulator");
//std::cout <<"robot trajectory is created"<<std::endl;
//rt.setRobotTrajectoryMsg(*mgi->getCurrentState(), traj_msg);
// Second get a RobotTrajectory from trajectory
// std::cout << "robot trajectory message"<<std::endl;

    // Thrid create a IterativeParabolicTimeParameterization object
    //trajectory_processing::IterativeParabolicTimeParameterization iptp;
    //std::cout << "iptp" <<std::endl;
    // Fourth compute computeTimeStamps
    //bool success = iptp.computeTimeStamps(rt, max_velocity_scale, max_acceleration_scale);
    //rt.getRobotTrajectoryMsg(trajectory);

And I keep getting following error:

bpmpd_caller: /home/lakshmi/catkin_ws/src/trajopt_ros/trajopt_sco/include/trajopt_sco/bpmpd_io.hpp:35: void bpmpd_io::ser(int, T&, bpmpd_io::SerMode) [with T = char]: Assertion `n == sizeof(T)' failed.
Segmentation fault (core dumped)

Kindly please help me if I am wrong.

[lakshmip001]

Hi, I was able to solve the error and was able to obtain velocities and accelerations by launching my files separately(planning_execution.launch).

But I am unable to achieve a trapezoidal velocity with following cartvelterminfo()
std::shared_ptr cv(new CartVelTermInfo);
cv->term_type = TT_COST;
cv->first_step = 0;
cv->last_step = pci.basic_info.n_steps-1;
cv->max_displacement = 0.05;
cv->link = "tool0";
I get following profile

Kindly please help me in understanding this parameters to acheive trapeziodal velocity.