def __init__(self,
vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault",
drake_sample_rate=4.0,
drake_orientation_type=2,
drake_link_axis=[0, 0, 1]):
'''Constructor'''
super(PlanAffordanceState, self).__init__(
outcomes=['done', 'incomplete', 'failed'],
input_keys=['affordance', 'hand', 'reference_point'],
output_keys=['joint_trajectory', 'plan_fraction'])
self._client = ProxyMoveitClient()
try:
self.set_up_affordance_service() # topics and clients are set here
except Exception as e:
Logger.logerr(
'Failed to set up GetAffordanceInWristFrame service: \n%s' %
str(e))
return
self._vel_scaling = vel_scaling
self._planner_id = planner_id
self._drake_sample_rate = drake_sample_rate
self._drake_orientation_type = drake_orientation_type
self._drake_link_axis = Point(x=drake_link_axis[0],
y=drake_link_axis[1],
z=drake_link_axis[2])
self._failed = False
self._done = False
self._incomplete = False
def __init__(self,
ignore_collisions=False,
include_torso=False,
keep_endeffector_orientation=False,
allow_incomplete_plans=False,
vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"):
'''Constructor'''
super(PlanEndeffectorCartesianWaypointsState,
self).__init__(outcomes=['planned', 'incomplete', 'failed'],
input_keys=['waypoints', 'hand', 'frame_id'],
output_keys=['joint_trajectory', 'plan_fraction'])
self._client = ProxyMoveitClient()
self._ignore_collisions = ignore_collisions
self._include_torso = include_torso
self._keep_endeffector_orientation = keep_endeffector_orientation
self._allow_incomplete_plans = allow_incomplete_plans
self._vel_scaling = vel_scaling
self._planner_id = planner_id
self._failed = False
self._done = False
self._incomplete = False
def __init__(self, planning_group):
"""Constructor"""
super(MoveitMoveGroupState,
self).__init__(outcomes=['reached', 'failed'],
input_keys=['target_joint_config'])
self._client = ProxyMoveitClient()
self._failed = False
self._planning_group = planning_group
def __init__(self, planning_group, vel_scaling=0.1):
"""Constructor"""
super(MoveitPlanningState,
self).__init__(outcomes=['planned', 'failed'],
input_keys=['joint_values'],
output_keys=['joint_trajectory'])
self._client = ProxyMoveitClient()
self._planning_group = planning_group
self._vel_scaling = vel_scaling
self._failed = False
def __init__(self, ignore_collisions = False, include_torso = False, keep_endeffector_orientation = False, planner_id = "RRTConnectkConfigDefault"): ''' Constructor ''' super(PlanEndeffectorCircularMotionState, self).__init__(outcomes=['planned', 'failed'], input_keys=['hand', 'rotation_axis', 'rotation_angle'], output_keys=['joint_trajectory', 'plan_fraction']) self._client = ProxyMoveitClient() self._ignore_collisions = ignore_collisions self._include_torso = include_torso self._keep_endeffector_orientation = keep_endeffector_orientation self._failed = False self._planner_id = planner_id
def __init__(self,
target_pose,
vel_scaling=0.1,
ignore_collisions=False,
link_paddings={},
is_cartesian=False):
"""Constructor"""
super(MoveitPredefinedPoseState,
self).__init__(outcomes=['done', 'failed'], input_keys=['side'])
if not rospy.has_param("behavior/robot_namespace"):
Logger.logerr(
"Need to specify parameter behavior/robot_namespace at the parameter server"
)
return
self._robot = rospy.get_param("behavior/robot_namespace")
self._poses = dict()
self._poses['flor'] = dict()
# Position mode widget: src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch
self._poses['flor']['left'] = {
# Joint names: l_arm_shz, l_arm_shx, l_arm_ely, l_arm_elx, l_arm_wry, l_arm_wrx, l_arm_wry2
1: {
'group': 'l_arm_group',
'joints': [-0.25, -1.34, 1.88, 0.49, 0.0, 0.0, 0.0]
},
3: {
'group': 'l_arm_group',
'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4, -0.50]
},
10: {
'group': 'l_arm_group',
'joints': [-1.0, 0.28, 1.2, 1.6, 0.3, 0.5, 0.0]
},
21: {
'group': 'torso_group',
'joints': [+0.20, 0.00, 0.00]
},
22: {
'group': 'torso_group',
'joints': [+0.40, 0.00, 0.00]
},
23: {
'group': 'torso_group',
'joints': [+0.55, 0.00, 0.00]
},
112: {
'group': 'l_leg_group',
'joints': [+0.00, +0.00, -1.60, +1.40, -0.50, 0.00]
}, # Safety pose
114: {
'group': 'l_arm_group',
'joints': [+0.76, -0.94, 0.80, 2.00, +1.00, -0.20, -1.35]
}, # pre_drive
115: {
'group': 'l_arm_group',
'joints': [+0.11, -0.16, 1.75, 1.60, +1.00, -0.90, -1.00]
}, # drive
116: {
'group': 'l_arm_group',
'joints': []
}, # We use the right hand for the camera!
131: {
'group': 'l_arm_group',
'joints': [-0.29, -0.22, 1.87, 2.17, -0.17, 0.81, 0.12]
},
132: {
'group': 'l_arm_group',
'joints': [-0.70, -0.09, 1.93, 0.66, -0.15, 1.52, 0.12]
},
133: {
'group': 'l_arm_group',
'joints': [-1.38, -0.16, 1.82, 0.57, -0.19, 1.52, 0.12]
},
134: {
'group': 'l_arm_group',
'joints': []
}, # Most probably will never be used
151: {
'group': 'l_arm_group',
'joints': [-1.01, -0.43, +1.32, +1.67, -0.91, +1.46, +0.98]
},
155: {
'group': 'l_arm_group',
'joints': [0.0, -0.37, 2.65, 1.4, -0.2, 0.9, -1.54]
},
156: {
'group': 'l_arm_group',
'joints': [-0.32, -0.9, 2.2, 1.3, 0.5, 1.0, -1.8]
},
157: {
'group': 'l_arm_group',
'joints': [-0.45, -1.0, 2.1, 1.3, 0.5, 0.8, -0.8]
}
}
self._poses['flor']['same'] = {
0: {'group': 'both_arms_group', 'joints': [-0.25, -1.34, +1.88, +0.49, +0.00, +0.00, +0.00, \
+0.25, +1.34, +1.88, -0.49, +0.00, +0.00, +0.00]},
2: {'group': 'both_arms_group', 'joints': [+0.72, -0.95, 2.7, 0.95, 0.0, -0.4 , -0.5, \
-0.72, 0.95, 2.7, -0.95, 0.0, 0.4 , -0.5]},
19: {'group': 'both_arms_group', 'joints': [-1.5, -1.5, +0.30, +0.50, +0.0, +0.8, +0.00, \
+1.5, +1.5, +0.30, -0.50, +0.0, -0.8, +0.00]},
20: {'group': 'torso_group', 'joints': [+0.00, 0.00, 0.00]},
111: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, +0.00, 0.0, \
+0.00, +1.50, +0.00, -1.72, 0.00, +0.00, 0.0]},
113: {'group': 'both_arms_group', 'joints': [+0.20, -1.50, +0.00, +1.72, 0.00, -1.57, 0.0, \
+0.00, +1.50, +0.00, -1.72, 0.00, +1.57, 0.0]},
181: {'group': 'both_arms_group', 'joints': [-1.53, -0.69, +0.12, +1.47, 0.00, +0.88, 0.00, \
+1.53, +0.69, +0.12, -1.47, 0.00, -0.88, 0.00]}
}
self._poses['flor']['right'] = {
# Joint names: r_arm_shz, r_arm_shx, r_arm_ely, r_arm_elx, r_arm_wry, r_arm_wrx, r_arm_wry2
1: {
'group': 'r_arm_group',
'joints': [+0.25, 1.34, 1.88, -0.49, 0.0, 0.0, 0.0]
},
3: {
'group': 'r_arm_group',
'joints': [-0.72, 0.95, 2.7, -0.95, 0.0, 0.4, -0.50]
},
10: {
'group': 'r_arm_group',
'joints': [+1.0, -0.28, 1.2, -1.6, 0.3, -0.5, 0.0]
},
21: {
'group': 'torso_group',
'joints': [-0.20, 0.00, 0.00]
},
22: {
'group': 'torso_group',
'joints': [-0.40, 0.00, 0.00]
},
23: {
'group': 'torso_group',
'joints': [-0.55, 0.00, 0.00]
},
112: {
'group': 'r_leg_group',
'joints': [+0.00, +0.00, -1.34, +1.23, 0.00, 0.00]
},
115: {
'group': 'r_arm_group',
'joints': []
}, # Driving is done with the left arm!
116: {
'group': 'r_arm_group',
'joints': [+0.90, 0.17, 0.50, -1.58, -0.70, +1.50, -0.34]
}, # CarCamera
131: {
'group': 'r_arm_group',
'joints': [+0.29, 0.22, 1.87, -2.17, -0.17, -0.81, 0.12]
},
132: {
'group': 'r_arm_group',
'joints': [+0.70, 0.09, 1.93, -0.66, -0.15, -1.52, 0.12]
},
133: {
'group': 'r_arm_group',
'joints': [+1.38, 0.16, 1.82, -0.57, -0.19, -1.52, 0.12]
},
134: {
'group': 'r_arm_group',
'joints': [+0.00, +0.54, +0.94, -1.04, 0.80, 0.5, 0.7]
},
151: {
'group': 'r_arm_group',
'joints': [+1.01, +0.43, +1.32, -1.67, -0.91, -1.46, +0.98]
},
155: {
'group': 'r_arm_group',
'joints': [+0.00, +0.37, +2.65, -1.40, -0.20, -0.90, -1.54]
},
156: {
'group': 'r_arm_group',
'joints': [+0.32, +0.90, +2.20, -1.30, +0.50, -1.00, -1.80]
},
157: {
'group': 'r_arm_group',
'joints': [0.45, 1.0, 2.1, -1.3, 0.5, -0.8, -0.8]
}
}
self._poses['thor_mang'] = dict()
self._poses['thor_mang']['left'] = {
1: {
'group':
'l_arm_group',
'joints': [
0.785385646194622, -0.281153767716932,
0.000600782658167331, -1.57080884130538, -0.25205140042963,
0.01563815008, 0
]
},
3: {
'group': 'l_arm_group',
'joints': [0.77, -0.27, 0.02, -1.21, -0.25, 0.02, 0]
},
4: {
'group':
'l_arm_group',
'joints': [
0.785385646194622, -0.281153767716932,
0.000600782658167331, -1.97080884130538, -0.25205140042963,
0.01563815008, 0
]
},
21: {
'group': 'torso_group',
'joints': [+0.30, 0.03]
},
22: {
'group': 'torso_group',
'joints': [+0.60, 0.03]
},
23: {
'group': 'torso_group',
'joints': [+1.02, 0.03]
},
31: {
'group': 'head_group',
'joints': [1.57, 0.00]
},
131: {
'group': 'l_arm_group',
'joints': [-1.73, -0.69, 1.75, -1.86, 0.04, -0.72, 1.63]
},
132: {
'group': 'l_arm_group',
'joints': [-1.76, -1.13, 1.68, -0.55, 0.02, -1.81, 1.63]
},
133: {
'group': 'l_arm_group',
'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]
},
134: {
'group': 'l_arm_group',
'joints': [-1.70, -0.06, 1.86, -0.45, 0.08, -0.90, 1.65]
}, # placeholder
136: {
'group': 'l_arm_group',
'joints': [-2.31, -2.2, 1.83, -1.22, 4.1, 0.14, -2.08]
},
138: {
'group': 'torso_group',
'joints': [0.92, 0]
},
139: {
'group': 'torso_group',
'joints': [1.3, 0]
},
151: {
'group': 'l_arm_group',
'joints': [-1.45, -1.26, 1.10, -2.02, -0.19, 0.04, -2.86]
},
155: {
'group': 'l_arm_group',
'joints': [-0.25, -0.75, 0.02, -1.21, -0.38, -1.36, -0.9]
},
156: {
'group': 'l_arm_group',
'joints': [-0.25, -0.25, 0.02, -1.21, -0.38, -1.36, -0.9]
},
157: {
'group': 'l_arm_group',
'joints': [-0.25, -0.13, 0.02, -1.21, -0.13, -1.55, 0.41]
}
}
self._poses['thor_mang']['same'] = {
0: {
'group':
'both_arms_group',
'joints': [
0.79, -0.27, 0, -1.57, 1.55, 0, 0, -0.79, 0.27, 0, 1.57,
-1.55, 0, 0
]
},
2: {
'group':
'both_arms_group',
'joints':
[-0.37, 0.0, 1.57, 0, 0, 0, 0, 0.37, 0.0, 1.57, 0, 0, 0, 0]
},
20: {
'group': 'torso_group',
'joints': [0.00, 0.03]
},
30: {
'group': 'head_group',
'joints': [0.00, 0.00]
},
181: {
'group':
'both_arms_group',
'joints': [
0.785385646194622, -0.281153767716932,
0.000600782658167331, -1.57080884130538, -0.25205140042963,
0.01563815008, 0, -0.785385646194622, 0.281153767716932,
-0.000600782658167331, 1.57080884130538, 0.25205140042963,
-0.01563815008, 0
]
}
}
self._poses['thor_mang']['right'] = {
1: {
'group':
'r_arm_group',
'joints': [
-0.785385646194622, 0.281153767716932,
-0.000600782658167331, 1.57080884130538, 0.25205140042963,
-0.01563815008, 0
]
},
3: {
'group': 'r_arm_group',
'joints': [-0.77, 0.27, -0.02, 1.21, 0.25, -0.02, 0]
},
4: {
'group':
'r_arm_group',
'joints': [
-0.785385646194622, 0.281153767716932,
-0.000600782658167331, 1.97080884130538, 0.25205140042963,
-0.01563815008, 0
]
},
21: {
'group': 'torso_group',
'joints': [-0.30, 0.03]
},
22: {
'group': 'torso_group',
'joints': [-0.60, 0.03]
},
23: {
'group': 'torso_group',
'joints': [-1.02, 0.03]
},
31: {
'group': 'head_group',
'joints': [-1.57, 0.00]
},
131: {
'group': 'r_arm_group',
'joints': [1.73, 0.69, -1.75, 1.86, -0.04, 0.72, -1.63]
},
132: {
'group': 'r_arm_group',
'joints': [1.76, 1.13, -1.68, 0.55, -0.02, 1.81, -1.63]
},
133: {
'group': 'r_arm_group',
'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]
},
134: {
'group': 'r_arm_group',
'joints': [1.70, 0.06, -1.86, 0.45, -0.08, 0.90, -1.65]
}, # placeholder
136: {
'group': 'r_arm_group',
'joints': [2.31, 2.2, -1.83, 1.22, -4.1, -0.14, 2.08]
},
138: {
'group': 'torso_group',
'joints': [-0.92, 0]
},
139: {
'group': 'torso_group',
'joints': [-1.3, 0]
},
151: {
'group': 'r_arm_group',
'joints': [1.45, 1.26, -1.10, 2.02, 0.19, -0.04, 2.86]
},
155: {
'group': 'r_arm_group',
'joints': [0.25, 0.75, -0.02, 1.21, 0.38, 1.36, 0.9]
},
156: {
'group': 'r_arm_group',
'joints': [0.25, 0.25, -0.02, 1.21, 0.38, 1.36, 0.9]
},
157: {
'group': 'r_arm_group',
'joints': [0.25, 0.13, -0.02, 1.21, 0.13, 1.55, -0.41]
}
}
self._target_pose = target_pose
self._vel_scaling = vel_scaling
self._ignore_collisions = ignore_collisions
self._link_paddings = link_paddings
self._is_cartesian = is_cartesian
self._client = ProxyMoveitClient()
self._failed = False
