Adding constraints on the manipulator and the external axis

[mohamedsayed18]

I'm trying to make the tool of the robot always point down and the external axis should rotate to compensate for this.
I thought about the cartesian_coeff but as I understand this will add cost/constraint to the generated path between two waypoints, not adding constraints on the robot itself.
I think the right choice is the joint_coeff by adding a high coefficient value for the joints I don't want to move a lot, but that seems like not working.
Any help would be appreciated, Note: I'm working on a 3D printing application.

[Levi-Armstrong]

In this case since the default trajopt plan profile does not provide what you need I would create a new profile. I would inherit form the TrajOptDefaultPlanProfile and override the apply method for the cartesian waypoint. In this function I would do exactly what it is doing so it adds the dynamic cartesian constraint or cost. Then add an additional cartesian constraint using the world frame as the desired frame and then set the coefficients to [0,0,0,5,5,0] which will create a constraint that it must be oriented with world but allowed to rotate around the world z-axis. You could also take it a step further if you are using descartes and create a custom sampler which only generates sampled states which satisfy this to provided a better seed to trajopt. I would only do this if trajopt has issues finding a solution.

[mohamedsayed18]

To constraint the targets to a specific alignment I was thinking about limiting the joint value of the end effector between two values.
can I use the upper_tols and lower_tols to achieve this?

or is it better to use a different method to achieve the targets' alignment?

[Levi-Armstrong]

The tolerance functionality in TrajOpt has had little testing and not sure it works as expected. If you plan to leverage tolerances I recommend using TrajOptIfopt.

[mohamedsayed18]

I did several trials with joint tolerance but it seems like not the right tool for my problem.
Regarding using the trajopt profile(I made a new one inherited from trajopt) to add a constraint to my problem, it restricts the waypoint from rotation around the world frame but it didn't restrict the waypoint to have a specific orientation.
In the following gif, I have the target with the z-axis pointing parallel to the external axis plate. this is the output without adding any constraints.

To add the constraint I added createCartesianWaypointTermInfo, with world as the working frame and the coefficients are [0,0,0,5,5,0]

as shown, the rotation of the waypoint is constrained with respect to the world. But what I was trying to achieve was to constraint the target to have a specific orientation(z-axis pointing down).
As an overview of the problem I'm trying to solve. The input is a cartesian waypoint (position and orientation with respect to the external axis). If the waypoint's z-axis is not pointing down with respect to the world frame, the external axis should change its orientation to achieve the desired orientation.

[Levi-Armstrong]

is the last waypoint in the trajectory the waypoint you are wanting to constrain? If that is the case then the positioner should rotate 90 degrees so it would be point down? Also, can you point me to what you currently have?

[mohamedsayed18]

is the last waypoint in the trajectory the waypoint you are wanting to constrain?

Yes, but the bigger goal is to make even the intermediate points constrained. considering this motion as continuous printing.

If that is the case then the positioner should rotate 90 degrees so it would be point down?

Yes, that is how I think we could keep the tool always pointing down.

can you point me to what you currently have?

Here I inherited the trajopt default profile and added an extra constraint for the cartesian waypoint.
But right now I think of creating a new TermInfo for example CreateAlignTermInfo which aligns the cartesian waypoint with the world, but I'm still thinking if it is possible to do so?