Significant deviance in IK computation

[denizkitu]

I've been trying to perform pick and place with MoveIt Task Constructor but the ik_frame and target_frame never seems to overlap as they should. I've made sure that the reference frame that ik solver uses is the gripper's frame. But i observed that none of the frames reach the target_frame. This is my code: (almost the same as in the panda example)

#! /usr/bin/env python

from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.task_constructor import core, stages
from moveit_commander import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, TwistStamped
import time

roscpp_initialize("pickplace")


arm = "arm_controller"
eef = "hand"

object_name = "grasp_object"
object_radius = 0.02

psi = PlanningSceneInterface(synchronous=True)
psi.remove_world_object()

objectPose = PoseStamped()
objectPose.header.frame_id = "world"
objectPose.pose.orientation.x = 1.0
objectPose.pose.position.x = 0.30702
objectPose.pose.position.y = 0.0
objectPose.pose.position.z = 0.285

psi.add_box(object_name, objectPose, size=[0.03, 0.03, 0.03])

task = core.Task("PandaPickPipelineExample")
task.enableIntrospection()

task.add(stages.CurrentState("current"))

pipeline = core.PipelinePlanner()
pipeline.planner = "RRTConnectkConfigDefault" 
planners = [(arm, pipeline)]


task.add(stages.Connect("connect", planners))

grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
grasp_generator.angle_delta = 0.2
grasp_generator.pregrasp = "open"
grasp_generator.grasp = "close"
grasp_generator.setMonitoredStage(task["current"])  


simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp")

ik_frame = PoseStamped()
ik_frame.header.frame_id = "gripper_link"
ik_frame.pose.position.z = 0.09 
ik_frame.pose.position.y = -0.02
simpleGrasp.setIKFrame(ik_frame)

pick = stages.Pick(simpleGrasp, "Pick")
pick.eef = eef
pick.object = object_name


approach = TwistStamped()
approach.header.frame_id = "world"
approach.twist.linear.z = -1

pick.setApproachMotion(approach, 0.04, 0.40)


lift = TwistStamped()
lift.header.frame_id = "gripper_link"
lift.twist.linear.z = -1.0
pick.setLiftMotion(lift, 0.03, 0.1)

task.add(pick)

if task.plan():
    task.publish(task.solutions[0])


del pipeline
del planners

time.sleep(3600)