From 460118d1e724a3a8ed6b43dcbda4c994c4204cfc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Be=C3=9Fler?= Date: Wed, 9 Dec 2015 14:48:00 +0100 Subject: [PATCH] object pose estimation during grasp actions --- knowrob_saphari/prolog/knowrob_saphari.pl | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/knowrob_saphari/prolog/knowrob_saphari.pl b/knowrob_saphari/prolog/knowrob_saphari.pl index 8d902c95..ebe14170 100644 --- a/knowrob_saphari/prolog/knowrob_saphari.pl +++ b/knowrob_saphari/prolog/knowrob_saphari.pl @@ -93,6 +93,7 @@ :- use_module(library('comp_temporal')). :- use_module(library('knowrob_mongo')). :- use_module(library('knowrob_marker')). +:- use_module(library('knowrob_math')). :- use_module(library('srdl2')). :- use_module(library('knowrob_cram')). :- use_module(library('knowrob_objects')). @@ -349,8 +350,10 @@ rdf_has(Event, knowrob:'objectActedOn', Designator), mng_lookup_transform('/map', '/gripper_tool_frame', T, Matrix), matrix_translation(Matrix,Translation), - % TODO: orientation not correct - matrix_rotation(Matrix,Orientation), !. + matrix_rotation(Matrix,GripperOrientation), + % Apply offset quaternion + quaternion_multiply(GripperOrientation, + [0.0,0.7071067811865476,-0.7071067811865475,0.0], Orientation), !. saphari_object_pose_estimate(_, PoseIn, PoseIn). :- knowrob_marker:marker_transform_estimation_add(knowrob_saphari:saphari_object_pose_estimate). @@ -491,14 +494,14 @@ saphari_active_task(Task) :- % for now assume a task is active when no endTime asserted rdf_has(Task, rdf:type, saphari:'SaphariTaskDescription'), - not( rdf_has(Task, knowrob:endTime, _) ). + not( rdf_has(Task, knowrob:endTime, _) ), !. saphari_active_task(Task, T) :- time_term(T, T_term), rdf_has(Task, rdf:type, saphari:'SaphariTaskDescription'), event_interval(Task, T0, T1), T_term >= T0, - T_term =< T1. + T_term =< T1, !. % Find list of empty slots with corresponding desired object classes for the slots saphari_empty_slot((SlotId, ObjectClass, Pose)) :-