Skip to content

Commit c814ac7

Browse files
author
Marcello
committed
the robot now moves and follows the mouse when the user presses the keybord key 'l'. At the moment, it only works for the right hand.
1 parent a534d8d commit c814ac7

File tree

3 files changed

+83
-86
lines changed

3 files changed

+83
-86
lines changed

script/blenderRCBPanel/__init__.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@
3434
WM_OT_Connect,
3535
WM_OT_Configure,
3636
WM_OT_ReachTarget,
37-
#ModalOperator,
37+
ModalOperator,
3838
OBJECT_PT_robot_controller,
3939
OT_OpenConfigurationFile,
4040
ListItem,
@@ -51,7 +51,7 @@
5151
WM_OT_Connect,
5252
WM_OT_Configure,
5353
WM_OT_ReachTarget,
54-
#ModalOperator,
54+
ModalOperator,
5555
OBJECT_PT_robot_controller,
5656
OT_OpenConfigurationFile,
5757
ListItem,

script/blenderRCBPanel/blenderRCBPanel.py

Lines changed: 66 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,6 @@
1717
IkVariables as ikv,
1818
InverseKinematics,
1919
)
20-
#inverseKinematics,
21-
#dynComp,
22-
#iDynTreeModel)
2320

2421
from bpy_extras.io_utils import ImportHelper
2522
from bpy_extras import view3d_utils
@@ -40,13 +37,6 @@
4037
UIList
4138
)
4239

43-
# global variables
44-
# inverseKinematics = iDynTree.InverseKinematics()
45-
# dynComp = iDynTree.KinDynComputations()
46-
# iDynTreeModel = None
47-
48-
mouse_loc = [0, 0, 0]
49-
5040
list_of_links = []
5141

5242
# def printError(self, *args):
@@ -489,70 +479,71 @@ def execute(self, context):
489479
# bpy.ops.object.modal_operator('INVOKE_DEFAULT')
490480
return InverseKinematics.execute(self)
491481

492-
#
493-
# class ModalOperator(bpy.types.Operator):
494-
# bl_idname = "object.modal_operator"
495-
# bl_label = "Simple Modal Operator"
496-
497-
# def __init__(self):
498-
# self.mouse_pos = [0.0, 0.0]
499-
# self.object = None
500-
# print("Start")
501-
#
502-
# def __del__(self):
503-
# print("End")
504-
#
505-
# def execute(self, context):
506-
# print("location: ", mouse_loc[0], mouse_loc[1], mouse_loc[2])
507-
# return {'FINISHED'}
508-
#
509-
# def modal(self, context, event):
510-
# if event.type == 'L': # Apply
511-
# print("getting the locations")
512-
# global mouse_loc
513-
# self.mouse_pos = [event.mouse_region_x, event.mouse_region_y]
514-
#
515-
# self.object = bpy.context.object
516-
# region = bpy.context.region
517-
# region3D = bpy.context.space_data.region_3d
518-
# #The direction indicated by the mouse position from the current view
519-
# view_vector = view3d_utils.region_2d_to_vector_3d(region, region3D, self.mouse_pos)
520-
# #The 3D location in this direction
521-
# mouse_loc = view3d_utils.region_2d_to_location_3d(region, region3D, self.mouse_pos, view_vector)
522-
# #The 3D location converted in object local coordinates
523-
# # mouse_loc = self.object.matrix_world.inverted() * mouse_loc
524-
#
525-
# self.execute(context)
526-
#
527-
# elif event.type == 'O':
528-
# print("O pressed")
529-
# return {'FINISHED'}
530-
# elif event.type in {'P'}: # Cancel
531-
# print("P pressed")
532-
# return {'CANCELLED'}
533-
#
534-
# return {'RUNNING_MODAL'}
535-
#
536-
# def invoke(self, context, event):
537-
# if context.area.type == 'VIEW_3D':
538-
# print("Operator invoked")
539-
# args = (self, context)
540-
# # self._handle = bpy.types.SpaceView3D.draw_handler_add(draw_callback_px, args, 'WINDOW', 'POST_PIXEL')
541-
#
542-
# # Keeps mouse position current 3D location and current object for the draw callback
543-
# # (not needed to make self attribute if you don't want to use the callback)
544-
# self.mouse_pos = [0, 0]
545-
#
546-
# global mouse_loc
547-
# mouse_loc = [0, 0, 0]
548-
#
549-
# self.object = context.object
550-
#
551-
# context.window_manager.modal_handler_add(self)
552-
# return {'RUNNING_MODAL'}
553-
# else:
554-
# self.report({'WARNING'}, "View3D not found, cannot run operator")
555-
# return {'CANCELLED'}
482+
483+
class ModalOperator(bpy.types.Operator):
484+
bl_idname = "object.modal_operator"
485+
bl_label = "Simple Modal Operator"
486+
487+
def __init__(self):
488+
self.mouse_pos = [0.0, 0.0]
489+
self.object = None
490+
self.loc_3d = [0.0, 0.0, 0.0]
491+
print("Start")
492+
493+
def __del__(self):
494+
print("End")
495+
496+
def execute(self, context):
497+
print("location: ", self.loc_3d[0], self.loc_3d[1], self.loc_3d[2])
498+
return {'FINISHED'}
499+
500+
def modal(self, context, event):
501+
if event.type == 'L': # Apply
502+
print("getting the locations")
503+
self.loc_3d = [event.mouse_region_x, event.mouse_region_y]
504+
505+
self.object = bpy.context.object
506+
region = bpy.context.region
507+
region3D = bpy.context.space_data.region_3d
508+
#The direction indicated by the mouse position from the current view
509+
view_vector = view3d_utils.region_2d_to_vector_3d(region, region3D, self.loc_3d)
510+
#The 3D location in this direction
511+
self.loc_3d = view3d_utils.region_2d_to_location_3d(region, region3D, self.loc_3d, view_vector)
512+
#The 3D location converted in object local coordinates
513+
# self.loc_3d = self.object.matrix_world.inverted() * mouse_loc
514+
515+
InverseKinematics.execute(self, self.loc_3d)
516+
517+
self.execute(context)
518+
519+
elif event.type == 'O':
520+
print("O pressed")
521+
return {'FINISHED'}
522+
elif event.type in {'P'}: # Cancel
523+
print("P pressed")
524+
return {'CANCELLED'}
525+
526+
return {'RUNNING_MODAL'}
527+
528+
def invoke(self, context, event):
529+
if context.area.type == 'VIEW_3D':
530+
print("Operator invoked")
531+
args = (self, context)
532+
# self._handle = bpy.types.SpaceView3D.draw_handler_add(draw_callback_px, args, 'WINDOW', 'POST_PIXEL')
533+
534+
# Keeps mouse position current 3D location and current object for the draw callback
535+
# (not needed to make self attribute if you don't want to use the callback)
536+
self.loc_3d = [0, 0]
537+
538+
self.loc_3d = [0, 0, 0]
539+
540+
self.object = context.object
541+
542+
context.window_manager.modal_handler_add(self)
543+
return {'RUNNING_MODAL'}
544+
else:
545+
self.report({'WARNING'}, "View3D not found, cannot run operator")
546+
return {'CANCELLED'}
556547

557548
# ------------------------------------------------------------------------
558549
# Panel in Object Mode
@@ -684,7 +675,7 @@ def parse_conf(self, filepath, context):
684675
def execute(self, context):
685676
filename, extension = os.path.splitext(self.filepath)
686677
self.parse_conf(self.filepath, context)
687-
#bpy.ops.object.modal_operator('INVOKE_DEFAULT')
678+
bpy.ops.object.modal_operator('INVOKE_DEFAULT')
688679
return {'FINISHED'}
689680

690681

script/blenderRCBPanel/common_functions.py

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ def __init__(self):
1919
pass
2020

2121
@staticmethod
22-
def execute(object):
22+
def execute(object, xyz=[], rpy=[]):
2323
#global inverseKinematics, dynComp
2424
scene = bpy.context.scene
2525
considered_joints = []
@@ -30,7 +30,7 @@ def execute(object):
3030
mytool = scene.my_tool
3131

3232
base_frame = mytool.my_baseframeenum
33-
endeffector_frame = mytool.my_eeframeenum
33+
endeffector_frame = 'r_hand' #mytool.my_eeframeenum
3434

3535
if base_frame == endeffector_frame:
3636
printError(object, "Base frame and end-effector frame are coincident!")
@@ -87,13 +87,19 @@ def execute(object):
8787

8888
# base_H_ee_initial = IkVariables.dynComp.getRelativeTransform(base_frame, endeffector_frame);
8989

90-
iDynTreeRotation = iDynTree.Rotation.RPY(mytool.my_reach_roll * math.pi / 180,
91-
mytool.my_reach_pitch * math.pi / 180,
92-
mytool.my_reach_yaw * math.pi / 180)
93-
94-
iDynTreePosition = iDynTree.Position(mytool.my_reach_x, mytool.my_reach_y, mytool.my_reach_z)
95-
# global mouse_loc
96-
# iDynTreePosition = iDynTree.Position(mouse_loc[0], mouse_loc[1], mouse_loc[2])
90+
if not rpy:
91+
iDynTreeRotation = iDynTree.Rotation.RPY(mytool.my_reach_roll * math.pi / 180,
92+
mytool.my_reach_pitch * math.pi / 180,
93+
mytool.my_reach_yaw * math.pi / 180)
94+
else:
95+
iDynTreeRotation = iDynTree.Rotation.RPY(rpy[0] * math.pi / 180,
96+
rpy[1] * math.pi / 180,
97+
rpy[2] * math.pi / 180)
98+
99+
if not xyz:
100+
iDynTreePosition = iDynTree.Position(mytool.my_reach_x, mytool.my_reach_y, mytool.my_reach_z)
101+
else:
102+
iDynTreePosition = iDynTree.Position(xyz[0], xyz[1], xyz[2])
97103

98104
# Define the transform of the selected cartesian target
99105
base_H_ee_desired = iDynTree.Transform(iDynTreeRotation, iDynTreePosition)

0 commit comments

Comments
 (0)