def set_single_joint_position(self, joint_name, position, moving_time=None, accel_time=None, blocking=True):
self.set_trajectory_time(moving_time, accel_time)
self.joint_positions[self.joint_indx_dict[joint_name]] = position
single_command = SingleCommand(joint_name, position)
self.pub_single_command.publish(single_command)
if blocking:
rospy.sleep(self.moving_time)
self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_positions)
def set_single_joint_command(self,
joint_name,
command,
moving_time=None,
accel_time=None,
delay=0):
self.set_single_trajectory_time(joint_name, moving_time, accel_time)
single_command = SingleCommand(joint_name, command)
self.pub_single_command.publish(single_command)
if delay > 0:
rospy.sleep(delay)
def initialise_robot(self):
rospy.logdebug("===INITIALIZING ROBOT====")
moveit_commander.roscpp_initialize(sys.argv)
## This object is the outer-level interface to the robot:
robot = moveit_commander.RobotCommander()
## This object is an interface to one group of joints.
man_group_name = rospy.get_param('manipulator_group_name')
ee_group_name = rospy.get_param('ee_group_name')
rospy.logdebug("[MOVE ROBOT] Manipulator group name: %s", man_group_name)
rospy.logdebug("[MOVE ROBOT] End effector group name: %s", ee_group_name)
man_group = moveit_commander.MoveGroupCommander(man_group_name)
ee_group = moveit_commander.MoveGroupCommander(ee_group_name)
man_planning_frame = man_group.get_planning_frame()
ee_planning_frame = ee_group.get_planning_frame()
ee_link = man_group.get_end_effector_link()
rospy.logdebug("[MOVE ROBOT] Manipulator planning frame: %s", man_planning_frame)
rospy.loginfo("[MOVE ROBOT] Manipulator end effector link: %s", ee_link) # px150/ee_arm_link
rospy.logdebug("[MOVE ROBOT] End effector planning frame: %s", ee_planning_frame)
manipulator_joint_names = man_group.get_joints()
ee_joint_names = ee_group.get_named_target_values("Closed").keys() # the ee_group contains joints we cannot actually control?
EE_CLOSED = ee_group.get_named_target_values("Closed").values()
EE_OPEN = ee_group.get_named_target_values("Open").values()
# Allow replanning to increase the odds of a solution
man_group.allow_replanning(True)
# Allow 5 seconds per planning attempt
man_group.set_planning_time(5)
man_group.set_max_velocity_scaling_factor(0.8)
man_group.set_max_acceleration_scaling_factor(0.7)
# Allow some leeway in position (meters) and orientation (radians) affects PLANNING!
man_group.set_goal_position_tolerance(0.005)
man_group.set_goal_orientation_tolerance(np.deg2rad(0.5))
man_group.set_goal_joint_tolerance(np.deg2rad(0.5))
#
# ee_group.set_goal_position_tolerance(self.position_tol)
# ee_group.set_goal_orientation_tolerance(self.orientation_tol)
# ee_group.set_goal_joint_tolerance(self.ee_joint_tol)
if not self.simulation:
rospy.wait_for_service("get_robot_info")
srv_robot_info = rospy.ServiceProxy("get_robot_info", RobotInfo)
self.resp = srv_robot_info()
self.num_joints = self.resp.num_joints
self.gripper_index = self.num_joints + 1
self.set_operating_mode_srv = rospy.ServiceProxy('set_operating_modes', OperatingModes)
self.pub_gripper_command = rospy.Publisher("single_joint/command",
SingleCommand, queue_size=10, latch=True)
self.sub_joint_states = rospy.Subscriber("joint_states", JointState, self.joint_state_cb)
self.gripper_command = SingleCommand()
self.gripper_command.joint_name = 'gripper'
self.gripper_command.cmd = 0
self.joint_states = JointState()
self.close_pwm_cmd = -250
self.open_pwm_cmd = 250
self.man_group_name = man_group_name
self.ee_group_name = ee_group_name
self.robot = robot
self.man_group = man_group
self.ee_group = ee_group
self.man_planning_frame = man_planning_frame
self.ee_planning_frame = ee_planning_frame
self.ee_link = ee_link
self.ee_joint_names = ee_joint_names
self.EE_OPEN = EE_OPEN
self.EE_CLOSED = EE_CLOSED