def __init__(self, side_prefix):
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers')
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here',
callback=self.move_to_pose_cb)
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
# Code for moving the robots joints
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
def __init__(self, side_prefix):
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer('ik_request_markers_' +
self.side_prefix)
self._tf_listener = TransformListener()
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here',
callback=self.move_to_pose_cb)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_contoller_name = None
l_traj_contoller_name = None
if self.side_prefix == 'r':
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(
r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
else:
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(
l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.is_control_visible = False
self.ee_pose = self.get_ee_pose()
self.ik = IK(side_prefix)
self.update_viz()
self._menu_handler.apply(self._im_server,
'ik_target_marker_' + self.side_prefix)
self._im_server.applyChanges()
print self.ik
def __init__(self, side_prefix):
if GripperMarkers._tf_listener is None:
GripperMarkers._tf_listener = TransformListener()
self.ik = IK(side_prefix)
self.side_prefix = side_prefix
self._im_server = InteractiveMarkerServer(side_prefix + '_ik_request_markers')
self._menu_handler = MenuHandler()
self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)
self.is_control_visible = False
self.marker_pose = self._offset_pose(self.get_ee_pose(), -GripperMarkers._offset)
self.update_viz()
self._menu_handler.apply(self._im_server, 'ik_target_marker')
self._im_server.applyChanges()
def __init__(self):
#Setup inverse kinematics
self.ik = IK()
self.gripper = Robot_Gripper()
def BuildKineModules(self): assert (len(dh)>0,'No dh params found') assert( len(rho)==len(dh),'Specify all joint descriptions') self.IK=IK(self)
def __init__(self):
self.ik = IK()
self.plan_env = gym.make('PandaReacher-v0', shelf=True)
self.plan_env.reset()
self.p = self.plan_env.robot._p
self.drive_res = 100
