Extra constraints in motion planner #44

Hi,
I'm working on a project with a workpiece positioner robot like the one you can see here.

I have a coordinate in the local coordinate system of the robot together with a vector showing the orientation of that coordinate.
My goal is to rotate the robot joints so that the vector orientation of the coordinate is pointing down (vertical).

Is there a way to do this using the motion planner / IK? I can achieve this currently but I'm using some maths to get the angle between the vector and an extra vector which is always pointing down (vertical) and I would like to automate it so that it choose the most efficient solution.

I know there is the possibility to add IK constraints to a motion planner object using the "plan_to_cartesian_objective" function but I don´t think this is the right solution.

Anyone who can point me to the right direction?

Kind regards,

Benoît Lagasse

IK is your best bet. If you want to keep the end effector position constant between the start and end, you can do it as follows

from klampt import ik
from klampt.math import vectorops

tool_center_point = [0,0,0]  #whatever you want here
local_axis = [1,0,0]         #whatever axis you have in the local frame
world_axis = [0,0,-1]        #target axis in the world frame
end_effector_link = robot.link(robot.numLinks()-1)   #last link

#use forward kinematics to get the current world position of the tool center point
robot.setConfig(q_current)   #The current configuration of the arm
current_point = end_effector_link.getWorldPosition(tool_center_point)

#set up the IK objective and solve
obj = ik.objective(end_effector_link,local=[tool_center_point,vectorops.add(tool_center_point,local_axis)],world=[current_point,vectorops.madd(current_point,world_axis)])
#robot.setConfig(qinit)  #optional: set an initialization configuration for the IK solver
res = ik.solve(obj)
if not res:
    print("IK failed")
q_target = robot.getConfig()   #this is your result

It may also be worthwhile to build a Robot Interface Layer interface to your robot, since the Klampt utilities will automatically perform motion generation to perform smooth joint space motions and Cartesian motions.