def __init__(self, Fg=[90.06, -42.79, -52.23], F_thresh=10.0, K=0.015): rospy.init_node('ft_feedback_control') self.Fg = np.array(Fg) self.F_thresh = F_thresh self.K = K self.Tmax = 2.0 self.js = None self.wind_up = 0 self.IK = IK('ceiling', 'blue_robotiq_ft_frame_id', solve_type='Distance') self.robot = moveit_commander.RobotCommander() self.group = moveit_commander.MoveGroupCommander('blue_arm') self.listener = tf.TransformListener() rospy.Subscriber("/blue/joint_states", JointState, self.joint_callback) rospy.Subscriber("/robotiq_ft_wrench", WrenchStamped, self.ft_callback, queue_size=1) self.client = actionlib.SimpleActionClient( 'blue/follow_joint_trajectory', FollowJointTrajectoryAction) rospy.spin()
def __init__(self): urdf = rospy.get_param('/robot_description') self.ik_right = IK("base_link", "ee_link", 0.005, 1e-5, "Distance", urdf) #self.ik_left = IK("torso_lift_link", # "l_wrist_roll_link") #self.left_command = rospy.Publisher('/l_arm_controller/command', # JointTrajectory, # queue_size=1) self.right_command = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=1) #self.last_left_pose = None #self.left_pose = rospy.Subscriber('/left_controller_as_posestamped', #PoseStamped, #self.left_cb, queue_size=1) self.last_right_pose = None self.right_pose = rospy.Subscriber( '/free_positioning/gripper_marker_pose', PoseStamped, self.right_cb, queue_size=1) rospy.sleep(2.0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_goal_pose_planner_node') self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander('kuka_arm') #Reference link: https://answers.ros.org/question/255792/change-planner-in-moveit-python/ #self.group.set_planner_id('RRTstarkConfigDefault') self.group.set_planner_id('RRTConnectkConfigDefault') self.group.set_planning_time(10) self.group.set_num_planning_attempts(3) #self.group.set_planning_time(15) #self.group.set_num_planning_attempts(1) self.zero_joint_states = np.zeros(7) self.left_home_pose = False if self.left_home_pose: self.home_joint_states = np.array([ 0.0001086467455024831, 0.17398914694786072, -0.00015721925592515618, -1.0467143058776855, 0.0006054198020137846, -0.00030679398332722485, 3.3859387258416973e-06 ]) else: self.home_joint_states = np.array([ 0.0001086467455024831, -0.17398914694786072, 0.00015721925592515618, 1.0467143058776855, 0.0006054198020137846, -0.00030679398332722485, 3.3859387258416973e-06 ]) self.ik_solver = IK('world', 'allegro_mount') self.seed_state = [0.0] * self.ik_solver.number_of_joints
def __init__(self): self.ik_right = IK("torso_lift_link", #"r_wrist_roll_link") "r_gripper_tool_frame") self.ik_left = IK("torso_lift_link", "l_wrist_roll_link") self.left_command = rospy.Publisher('/l_arm_controller/command', JointTrajectory, queue_size=1) self.right_command = rospy.Publisher('/r_arm_controller/command', JointTrajectory, queue_size=1) self.last_left_pose = None self.left_pose = rospy.Subscriber('/left_controller_as_posestamped', PoseStamped, self.left_cb, queue_size=1) self.last_right_pose = None self.right_pose = rospy.Subscriber('/right_controller_as_posestamped', PoseStamped, self.right_cb, queue_size=1) rospy.sleep(2.0)
def trac_ik_solve(limb, ps): print 'publish', ps target_topic.publish(ps) local_base_frame = limb + "_arm_mount" ik_solver = IK(local_base_frame, limb + "_wrist", urdf_string=urdf_str) seed_state = [0.0] * ik_solver.number_of_joints # canonical pose in local_base_frame #hdr = Header(stamp=rospy.Time.now(), frame_id=from_frame) #ps = PoseStamped( # header=hdr, # pose=pose, # ) p = translate_frame(ps, local_base_frame) #print 'translated frame',p soln = ik_solver.get_ik( seed_state, p.pose.position.x, p.pose.position.y, p.pose.position.z, # X, Y, Z p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w, # QX, QY, QZ, QW 0.01, 0.01, 0.01, 0.1, 0.1, 0.1, ) print 'trac soln', soln return soln
def _initialize_ik(self, urdf_path): if not USE_TRACK_IK: self.ik_solver = None return from trac_ik_python.trac_ik import IK # killall -9 rosmaster base_link = get_link_name( self.robot, parent_link_from_joint(self.robot, self.arm_joints[0])) tip_link = get_link_name(self.robot, child_link_from_joint(self.arm_joints[-1])) # limit effort and velocities are required # solve_type: Speed, Distance, Manipulation1, Manipulation2 # TODO: fast solver and slow solver self.ik_solver = IK(base_link=str(base_link), tip_link=str(tip_link), timeout=0.01, epsilon=1e-5, solve_type="Speed", urdf_string=read(urdf_path)) if not CONSERVITIVE_LIMITS: return lower, upper = self.ik_solver.get_joint_limits() buffer = JOINT_LIMITS_BUFFER * np.ones(len(self.ik_solver.joint_names)) lower, upper = lower + buffer, upper - buffer lower[6] = -MAX_FRANKA_JOINT7 upper[6] = +MAX_FRANKA_JOINT7 self.ik_solver.set_joint_limits(lower, upper)
def get_ik(position): """Get the inverse kinematics Get the inverse kinematics of the UR5 robot using track_IK package giving a desired intial position Arguments: position {list} -- A position representing x, y and z Returns: sol {list} -- Joint angles or None if track_ik is not able to find a valid solution """ camera_support_angle_offset = 0.0 q = quaternion_from_euler(0.0, -3.14 + camera_support_angle_offset, 0.0) # Joint order: # ('shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link', 'grasping_link') ik_solver = IK("base_link", "grasping_link", solve_type="Distance") sol = ik_solver.get_ik([ 0.2201039360819781, -1.573845095552878, -1.521853400505349, -1.6151347051274518, 1.5704492904506875, 0.0 ], position[0], position[1], position[2], q[0], q[1], q[2], q[3]) if sol is not None: sol = list(sol) sol[-1] = 0.0 else: print("IK didn't return any solution") return sol
def __init__(self): self.robot = URDF.from_parameter_server() self.kdl_kin = KDLKinematics(self.robot, "base_link", "end1") self.end_pos_e = np.array([0, 0, 0, 1]) self.end_pos_e = self.end_pos_e.reshape(self.end_pos_e.shape[0], 1) self.ik_solver = IK("base_link", "end1") self.seed_state = [0.0] * self.ik_solver.number_of_joints
def __init__(self, urdf_str): self.urdf_tree = URDF.from_xml_string(urdf_str) base_link = "link0" nlinks = len(self.urdf_tree.link_map.keys()) end_link = "link{}".format(nlinks - 1) print("Link chain goes from {} to {}".format(base_link, end_link)) self.kdl_kin = KDLKinematics(self.urdf_tree, base_link, end_link) self.ik_solver = IK(base_link, end_link, urdf_string=urdf_str)
def __init__(self): # rospy.init_node('grasp_kdl') self.robot = URDF.from_parameter_server() base_link = 'world' end_link = 'palm_link' self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) self.ik_solver = IK(base_link, end_link) self.seed_state = [0.0] * self.ik_solver.number_of_joints
def __init__(self): """ Shouldn't need inputs. Stores: 2 move groups: arm and hand - moveit commander interface planning scene interface and publisher robot commander IK solver (using trac_ik) FK solver (moveit service) """ rospy.loginfo("To stop project CTRL + C") rospy.on_shutdown(self.shutdown) # Initialize moveit_commander # I honestly don't think it actually uses the sys.argv argument, # but I need to do a little more digging first. It parses the input # for arguments containing "__name:=" which it passes to another initializer # I need to dig further to see what the name parameter actually changes moveit_commander.roscpp_initialize(sys.argv) # Instatiate the move group self.group = moveit_commander.MoveGroupCommander('arm') self.group.set_planning_time(5) self.group.set_workspace([-3, -3, -3, 3, 3, 3]) # This publishes trajectories to RVIZ for visualization # Need to figure out how to get rid of the visualization as well # Also how to use this to publish individual poses self.display_planned_path_publisher = rospy.Publisher( 'arm/display_planned_path', DisplayTrajectory, queue_size=10) # This publishes updates to the planning scene self.planning_scene_publisher = rospy.Publisher('/collision_object', CollisionObject, queue_size=10) # Planning scene self.scene = moveit_commander.PlanningSceneInterface() # RobotCommander self.robot = moveit_commander.RobotCommander() # IK Solver with trac_ik # NOTE: Get this from the MoveGroup so it's adaptable to other robots self.ik_solver = IK('root', 'm1n6s300_end_effector') self.ik_default_seed = [0.0] * self.ik_solver.number_of_joints # FK Solver rospy.wait_for_service('/compute_fk') self.fk_solver = rospy.ServiceProxy('/compute_fk', GetPositionFK) rospy.sleep(2) # Rate is 10 Hz. Is this a good rate? rate = rospy.Rate(10)
def __init__(self): rospy.init_node('gcode_generator', anonymous='True') self.rospack = rospkg.RosPack() self.ar3_path = self.rospack.get_path('ar3') self.program_buffer = [] self.expected_len = 0 self.previous_pose = [0.0] * 6 with open(self.ar3_path + '/urdf/ar3.urdf', 'r') as fp: urdf = fp.read() self.solver = IK("world", "tcp", urdf_string=urdf)
def __init__(self): self.ik_solver = IK("world", "link4", timeout=0.1, solve_type="Distance") self.start_state = [0.0] * (self.ik_solver.number_of_joints - 1) self.bx = self.by = self.bz = 0.001 self.brx = self.bry = self.brz = 0.1 self.x, self.y, self.z, self.qx, self.qy, self.qz, self.qw = 0, 0, 0, 0, 0, 0, 1 rospy.init_node("ik_solver") self.ik_solution_pub = rospy.Publisher("/arm/move_jp", JointState, queue_size=10) self.position_for_controller = JointState() self.ROS_main_loop()
def trac_ik_solve(limb, ps): try: arm = baxter_interface.Limb(limb) state = arm.joint_angles() print 'solve current:', arm.joint_angles() jointnames = ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2'] command = [] for i in range(0, len(jointnames)): key = limb + "_" + jointnames[i] command.append(state[key]) print 'candidate seed', command local_base_frame = limb + "_arm_mount" ik_solver = IK( local_base_frame, limb + "_wrist", #limb+"_gripper", urdf_string=urdf_str) #seed_state = [0.0] * ik_solver.number_of_joints seed_state = command # canonical pose in local_base_frame #hdr = Header(stamp=rospy.Time.now(), frame_id=from_frame) #ps = PoseStamped( # header=hdr, # pose=pose, # ) #gripper_ps = translate_in_frame(ps,'right_wrist',0,0,0) #gripper_ps = translate_pose_in_own_frame(ps,'gripper_target',0.015,-0.02,-0.2) #gripper_ps = translate_pose_in_own_frame(ps,'gripper_target',0,0,0.05) p = translate_frame(ps, local_base_frame) #print 'translated frame',p soln = ik_solver.get_ik( seed_state, p.pose.position.x, p.pose.position.y, p.pose.position.z, # X, Y, Z p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w, # QX, QY, QZ, QW #0.01,0.01,0.01, 0.1, 0.1, 0.1, 0.3, 0.3, 0.3, ) print 'trac soln', soln return soln except: return None
def __init__( self ): # gets URDF from /robot_description parameter on param server rospy.init_node('trac_ik_py') rospy.loginfo("Kinematic thingy initializin' here...") self.ik_solver = IK("base_link", "Link_6") self.seed_state = [0.0] * self.ik_solver.number_of_joints self.publisher = rospy.Publisher('/joint_states', JointState, queue_size=10) self.seq = 0 self.z_offset = 0.1575 self.scale = 1 self.x = self.y = self.z = self.qx = self.qy = self.qz = self.qw = 0 # keep track of EE pose
def __init__(self, from_rosparam=False): if from_rosparam: try: urdf = rospy.get_param('/robot_description') except: urdf = open("modello.urdf").read() else: urdf = open("modello.urdf").read() urdf = urdf.replace('encoding="utf-8"', '').replace('encoding=\'utf-8\'', '') self.chain = kp.build_chain_from_urdf(urdf) self.njoints = len( self.chain.get_joint_parameter_names() ) self.end_link = "end_effector" # "link{}".format(self.njoints) self.ik_solver = IK("link0", self.end_link, urdf_string=urdf)
def _init_ik_solver(self, robot_urdf): self.kdl = ur_kinematics(robot_urdf, ee_link=self.ee_link) if self.ik_solver == IKFAST: # IKfast libraries if robot_urdf == 'ur3_robot': self.arm_ikfast = ur_ikfast.URKinematics('ur3') elif robot_urdf == 'ur3e_robot': self.arm_ikfast = ur_ikfast.URKinematics('ur3e') elif self.ik_solver == TRAC_IK: self.trac_ik = IK(base_link="base_link", tip_link=self.ee_link, timeout=0.001, epsilon=1e-5, solve_type="Speed", urdf_string=utils.load_urdf_string('ur_pykdl', robot_urdf)) else: raise Exception("unsupported ik_solver", self.ik_solver)
def __init__(self, root_link=None, tip_link="left_arm_7_link"): self._urdf = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._urdf) self._base_link = self._urdf.get_root( ) if root_link is None else root_link self._tip_link = tip_link self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._joint_names = self._urdf.get_chain(self._base_link, self._tip_link, links=False, fixed=False) self._num_jnts = len(self._joint_names) # KDL self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) # trac_ik urdf_str = rospy.get_param('/robot_description') self.trac_ik_solver = IK(self._base_link, self._tip_link, urdf_string=urdf_str) self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_types = [] for jnt_name in self._joint_names: jnt = self._urdf.joint_map[jnt_name] if jnt.limit is not None: self.joint_limits_lower.append(jnt.limit.lower) self.joint_limits_upper.append(jnt.limit.upper) else: self.joint_limits_lower.append(None) self.joint_limits_upper.append(None) self.joint_types.append(jnt.type) self._default_seed = [ -0.3723750412464142, 0.02747996523976326, -0.7221865057945251, -1.69843590259552, 0.11076358705759048, 0.5450965166091919, 0.45358774065971375 ] # home_pos
class IKSolver(): def __init__(self): self.ik_solver = IK("world", "link4", timeout=0.1, solve_type="Distance") self.start_state = [0.0] * (self.ik_solver.number_of_joints - 1) self.bx = self.by = self.bz = 0.001 self.brx = self.bry = self.brz = 0.1 self.x, self.y, self.z, self.qx, self.qy, self.qz, self.qw = 0, 0, 0, 0, 0, 0, 1 rospy.init_node("ik_solver") self.ik_solution_pub = rospy.Publisher("/arm/move_jp", JointState, queue_size=10) self.position_for_controller = JointState() self.ROS_main_loop() # lb, up = self.ik_solver.get_joint_limits() def ik_find_solution(self, x, y, z, qx, qy, qz, qw): if self.x != x or self.y != y or self.z != z or self.qx != qx or self.qy != qy or self.qz != qz or self.qw != qw: self.sol = self.ik_solver.get_ik(self.curent_js.position, self.x, self.y, self.z, self.qx, self.qy, self.qz, self.qw, self.bx, self.by, self.bz, self.brx, self.bry, self.brz) self.x = x self.y = y self.z = z self.qx = qx self.qy = qy self.qz = qz self.qw = qw if self.sol != None: self.sol = [round(i, 2) for i in self.sol] print(self.sol) self.position_for_controller.position = self.sol self.position_for_controller.position.append(0.0) self.ik_solution_pub.publish(self.position_for_controller) else: print("No solution") def ik_callback(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z qx = pose.orientation.x qy = pose.orientation.y qz = pose.orientation.z qw = pose.orientation.w print(pose) self.ik_find_solution(x, y, z, qx, qy, qz, qw) def js_cb(self, js): self.curent_js.position = js.position[:-1] def ROS_main_loop(self): self.curent_js = JointState() rospy.Subscriber("/l_cont_pose", Pose, self.ik_callback) rospy.Subscriber("/arm/measured_js", JointState, self.js_cb) rospy.loginfo("Init done. IK plugin is working") rospy.spin()
class GraspKDL(): ''' Grasp KDL class. ''' def __init__(self): # rospy.init_node('grasp_kdl') self.robot = URDF.from_parameter_server() base_link = 'world' end_link = 'palm_link' self.kdl_kin = KDLKinematics(self.robot, base_link, end_link) self.ik_solver = IK(base_link, end_link) self.seed_state = [0.0] * self.ik_solver.number_of_joints def forward(self, q): pose = self.kdl_kin.forward(q) return pose def jacobian(self, q): J = self.kdl_kin.jacobian(q) return J def inverse(self, pose): ik_js = self.ik_solver.get_ik(self.seed_state, pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) if ik_js is None: rospy.logerr('No IK solution for grasp planning!') return None return ik_js
def __init__(self): """ Path Planner Class. References: dynamicreplanning.weebly.com, moveit python interface tutorial, trac_ik python tutorial jaco_manipulation """ rospy.loginfo("To stop project CTRL + C") rospy.on_shutdown(self.shutdown) # Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) # Instatiate the move group self.group = moveit_commander.MoveGroupCommander('arm') self.group.set_planning_time(5) self.group.set_workspace([-3, -3, -3, 3, 3, 3]) # This publishes trajectories to RVIZ for visualization self.display_planned_path_publisher = rospy.Publisher( 'arm/display_planned_path', DisplayTrajectory, queue_size=10) # This publishes updates to the planning scene self.planning_scene_publisher = rospy.Publisher('/collision_object', CollisionObject, queue_size=10) # Planning scene self.scene = moveit_commander.PlanningSceneInterface() # RobotCommander self.robot = moveit_commander.RobotCommander() # IK Solver with trac_ik # NOTE: Get this from the MoveGroup so it's adaptable to other robots self.ik_solver = IK('root', 'm1n6s300_end_effector') self.ik_default_seed = [0.0] * self.ik_solver.number_of_joints # FK Solver rospy.wait_for_service('/compute_fk') self.fk_solver = rospy.ServiceProxy('/compute_fk', GetPositionFK) rospy.sleep(2) rate = rospy.Rate(10)
def __init__(self, urdf_param, LorR, pos_bound, ori_bound): # Get URDF from parameter server # self._urdf_str = rospy.get_param('/LArm/robot_description') self._urdf_str = rospy.get_param(urdf_param) # Create IK instance self._ik_solver = IK(LorR + "link0", LorR + "EEFlink", urdf_string=self._urdf_str) # set the Dof self._seed_state = [0.0] * self._ik_solver.number_of_joints #set the lower&upper bound self._lower_bound, self._upper_bound = self._ik_solver.get_joint_limits( ) #can modify the joint limits # self._ik_solver.set_joint_limits([0.0]* self._ik_solver.number_of_joints, upper_bound) self._pos_bound = pos_bound self._ori_bound = ori_bound
def __init__(self, hebiros_wrapper, urdf_str, base_link, end_link): """SMACH State :type hebiros_wrapper: HebirosWrapper :param hebiros_wrapper: HebirosWrapper instance for Leg HEBI group :type urdf_str: str :param urdf_str: Serialized URDF str :type base_link: str :param base_link: Leg base link name in URDF :type end_link: str :param end_link: Leg end link name in URDF """ State.__init__(self, outcomes=['ik_failed', 'success'], input_keys=[ 'prev_joint_pos', 'target_end_link_point', 'execution_time' ], output_keys=['prev_joint_pos', 'active_joints']) self.hebi_wrap = hebiros_wrapper self.urdf_str = urdf_str self.base_link = base_link self.end_link = end_link self.active = False # hardware interface self._hold_leg_position = True self._hold_joint_angles = [] self.hebi_wrap.add_feedback_callback(self._hold_leg_pos_cb) # pykdl self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link) self._active_joints = self.kdl_fk.get_joint_names() # trac-ik self.trac_ik = IK(base_link, end_link, urdf_string=urdf_str, timeout=0.01, epsilon=1e-4, solve_type="Distance") self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01] self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0 ] # NOTE: This implements position-only IK # joint state publisher while not rospy.is_shutdown() and len( self.hebi_wrap.get_joint_positions()) < len( self.hebi_wrap.hebi_mapping): rospy.sleep(0.1) self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
class ConbeIK(): def __init__(self, urdf_param, LorR, pos_bound, ori_bound): # Get URDF from parameter server # self._urdf_str = rospy.get_param('/LArm/robot_description') self._urdf_str = rospy.get_param(urdf_param) # Create IK instance self._ik_solver = IK(LorR + "link0", LorR + "EEFlink", urdf_string=self._urdf_str) # set the Dof self._seed_state = [0.0] * self._ik_solver.number_of_joints #set the lower&upper bound self._lower_bound, self._upper_bound = self._ik_solver.get_joint_limits( ) #can modify the joint limits # self._ik_solver.set_joint_limits([0.0]* self._ik_solver.number_of_joints, upper_bound) self._pos_bound = pos_bound self._ori_bound = ori_bound def check_setting(self): # check the info of model print('ik_solver.base_link : ', self._ik_solver.base_link) print('ik_solver.tip_link : ', self._ik_solver.tip_link) print('ik_solver.joint_names: ', self._ik_solver.joint_names) print('lower_bound : ', self._lower_bound) print('upper_bound : ', self._upper_bound) def calculate(self, pos, ori): #calculate IK result = self._ik_solver.get_ik( self._seed_state, pos[0], pos[1], pos[2], ori[0], ori[1], ori[2], ori[3], self._pos_bound[0], self._pos_bound[1], self._pos_bound[2], # X, Y, Z bounds self._ori_bound[0], self._ori_bound[1], self._ori_bound[2]) # Rotation X, Y, Z bounds return result
def __init__(self): # initialize publishers, subscribers, tflisteners self.arm_pub = rospy.Publisher('/goal_dynamixel_position', JointState, queue_size=1) self.pan_pub = rospy.Publisher('/pan/command', Float64, queue_size=1) self.tilt_pub = rospy.Publisher('/tilt/command', Float64, queue_size=1) self.gripper_open_pub = rospy.Publisher('/gripper/open', Empty, queue_size=1) self.gripper_close_pub = rospy.Publisher('/gripper/close', Empty, queue_size=1) rospy.Subscriber('/joint_states', JointState, self.get_joint_state) self.tf_listener = tf.TransformListener() self.history = {'timestamp': deque(), 'joint_feedback': deque()} # global variables self.current_joint_state = None self.current_gripper_state = None self.current_target_state = None # global frames self.GRIPPER_LINK = "gripper_link" self.BOTTOM_PLATE_LINK = "bottom_plate" # other classes self.ik_solver = IK(self.BOTTOM_PLATE_LINK, self.GRIPPER_LINK) # predefined variables self.HOME_POS_MANIPULATOR_00 = [0.0, 0.0, 0.0, 0.0, 0.0] self.HOME_POS_MANIPULATOR_01 = [ 0.004601942375302315, -0.4218447208404541, 1.6260197162628174, -0.1426602154970169, 0.010737866163253784 ] self.HOME_POS_MANIPULATOR_02 = [0.0, 0.0, 1.22, -1.57, 1.57] self.HOME_POS_CAMERA_01 = [0.0, 0.698] self.HOME_POS_CAMERA_02 = [-0.452, -0.452] self.IK_POSITION_TOLERANCE = 0.01 self.IK_ORIENTATION_TOLERANCE = np.pi / 3 self.MIN_CLOSING_GAP = 0.002 self.MOVEABLE_JOINTS = [0, 1, 4] self.CONVERGENCE_CRITERION = 0.1 self.CONVERGENCE_CRITERION_COUNT = 10
class RobotKinematics: """Calculate direct and inverse kinematics through different methods""" def __init__(self, from_rosparam=False): if from_rosparam: try: urdf = rospy.get_param('/robot_description') except: urdf = open("modello.urdf").read() else: urdf = open("modello.urdf").read() urdf = urdf.replace('encoding="utf-8"', '').replace('encoding=\'utf-8\'', '') self.chain = kp.build_chain_from_urdf(urdf) self.njoints = len( self.chain.get_joint_parameter_names() ) self.end_link = "end_effector" # "link{}".format(self.njoints) self.ik_solver = IK("link0", self.end_link, urdf_string=urdf) def calculate_direct_kinematic(self, joint_angles): assert len(joint_angles) == self.njoints joint_state = {} for i in range(0, len(joint_angles)): joint_state["joint{}".format(i)] = joint_angles[i] all_link_positions = self.chain.forward_kinematics(joint_state) return all_link_positions[self.end_link].pos def calculate_inverse_kinematics_ik(self, x, y, z): initial_state = np.array(self.njoints * [0.0]).astype(float) return self.ik_solver.get_ik(initial_state, x, y, z, 0, 0, 0, 1.0, 1e-8, 1e-8, 1e-8) def calculate_inverse_kinematics_optimization(self, x, y, z): goal_position = np.array([x, y, z]) print("Optimize ", goal_position) def distance_from_goal(joint_angles): current_position = self.calculate_direct_kinematic(joint_angles) return np.linalg.norm(goal_position - current_position) JOINT_LIMIT = (np.pi - 0.0000001)/4.0 N_JOINTS = self.njoints bounds = ( (-JOINT_LIMIT, JOINT_LIMIT), ) * N_JOINTS initial_state = np.array(N_JOINTS * [0.0]).astype(float) print("From initial state ", initial_state) solution = minimize(distance_from_goal, initial_state, method='SLSQP', bounds=bounds) return solution.x def get_joints_number(self): return self.njoints
def move_to_pose(limb, pose): pos, rot = pose #trac_ik kinematics intializing ik_solver = IK("base", "stp_021808TP00080_tip") current_joint_angles = limb.joint_angles() sorted_angles = sorted(current_joint_angles.items(), key=lambda kv: (kv[0], kv[1])) seed_state = [0] * ik_solver.number_of_joints for i in range(len(sorted_angles)): seed_state[i] = sorted_angles[i][1] kin = ik_solver.get_ik(seed_state, pos[0], pos[1], pos[2], rot[0], rot[1], rot[2], rot[3]) if kin == None: return None go_to_pose = dict(zip(limb.joint_names(), kin)) limb.move_to_joint_positions(go_to_pose)
class RobotKinematics: def __init__(self, urdf_str): self.urdf_tree = URDF.from_xml_string(urdf_str) base_link = "link0" nlinks = len(self.urdf_tree.link_map.keys()) end_link = "link{}".format(nlinks - 1) print("Link chain goes from {} to {}".format(base_link, end_link)) self.kdl_kin = KDLKinematics(self.urdf_tree, base_link, end_link) self.ik_solver = IK(base_link, end_link, urdf_string=urdf_str) def calculate_inverse_kinematics(self, joint_state, x, y, z, rx=0, ry=0, rz=0): return self.ik_solver.get_ik(joint_state, x, y, z, rx, ry, rz, 1.0, 1e-8, 1e-8, 1e-8) def calculate_inverse_kinematics_rl(self): # Deep Deterministic Policy gradients # https://blog.floydhub.com/robotic-arm-control-deep-reinforcement-learning/ pass def calculate_direct_kinematic(self, joint_state): forward_kin_matrix = self.kdl_kin.forward(joint_state) x = forward_kin_matrix[0, -1] y = forward_kin_matrix[1, -1] z = forward_kin_matrix[2, -1] return {"x": x, "y": y, "z": z} def get_joint_names(self): # return self.ik_solver.joint_names return self.urdf_tree.joint_map.keys() def get_link_names(self): # return self.ik_solver.link_names return self.urdf_tree.link_map.keys()
def ik_solver(X, Y, Z, QX, QY, QZ, QW): urdf_str = rospy.get_param('/robot_description') #print urdf_str ik_sol = IK("panda_link0","panda_link7",urdf_string=urdf_str) #print ik_sol.link_names global position seed_state = position[2:9] lower_bound, upper_bound = ik_sol.get_joint_limits() #print upper_bound #print lower_bound ik_sol.set_joint_limits(lower_bound, upper_bound) return ik_sol.get_ik(seed_state, X, Y, Z, # X, Y, Z QX, QY, QZ, QW) # QX, QY, QZ, QW
class World(object): def __init__(self, robot_name=FRANKA_CARTER, use_gui=True, full_kitchen=False): self.task = None self.interface = None self.client = connect(use_gui=use_gui) set_real_time(False) #set_caching(False) # Seems to make things worse disable_gravity() add_data_path() set_camera_pose(camera_point=[2, -1.5, 1]) if DEBUG: draw_pose(Pose(), length=3) #self.kitchen_yaml = load_yaml(KITCHEN_YAML) with HideOutput(enable=True): self.kitchen = load_pybullet(KITCHEN_PATH, fixed_base=True, cylinder=True) self.floor = load_pybullet('plane.urdf', fixed_base=True) z = stable_z(self.kitchen, self.floor) - get_point(self.floor)[2] point = np.array(get_point(self.kitchen)) - np.array([0, 0, z]) set_point(self.floor, point) self.robot_name = robot_name if self.robot_name == FRANKA_CARTER: urdf_path, yaml_path = FRANKA_CARTER_PATH, None #urdf_path, yaml_path = FRANKA_CARTER_PATH, FRANKA_YAML #elif self.robot_name == EVE: # urdf_path, yaml_path = EVE_PATH, None else: raise ValueError(self.robot_name) self.robot_yaml = yaml_path if yaml_path is None else load_yaml( yaml_path) with HideOutput(enable=True): self.robot = load_pybullet(urdf_path) #dump_body(self.robot) #chassis_pose = get_link_pose(self.robot, link_from_name(self.robot, 'chassis_link')) #wheel_pose = get_link_pose(self.robot, link_from_name(self.robot, 'left_wheel_link')) #wait_for_user() set_point(self.robot, Point(z=stable_z(self.robot, self.floor))) self.set_initial_conf() self.gripper = create_gripper(self.robot) self.environment_bodies = {} if full_kitchen: self._initialize_environment() self._initialize_ik(urdf_path) self.initial_saver = WorldSaver() self.body_from_name = {} # self.path_from_name = {} self.names_from_type = {} self.custom_limits = {} self.base_limits_handles = [] self.cameras = {} self.disabled_collisions = set() if self.robot_name == FRANKA_CARTER: self.disabled_collisions.update( tuple(link_from_name(self.robot, link) for link in pair) for pair in DISABLED_FRANKA_COLLISIONS) self.carry_conf = FConf(self.robot, self.arm_joints, self.default_conf) #self.calibrate_conf = Conf(self.robot, self.arm_joints, load_calibrate_conf(side='left')) self.calibrate_conf = FConf( self.robot, self.arm_joints, self.default_conf) # Must differ from carry_conf self.special_confs = [self.carry_conf] #, self.calibrate_conf] self.open_gq = FConf(self.robot, self.gripper_joints, get_max_limits(self.robot, self.gripper_joints)) self.closed_gq = FConf(self.robot, self.gripper_joints, get_min_limits(self.robot, self.gripper_joints)) self.gripper_confs = [self.open_gq, self.closed_gq] self.open_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.open_conf(joint)]) for joint in self.kitchen_joints } self.closed_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.closed_conf(joint)]) for joint in self.kitchen_joints } self._update_custom_limits() self._update_initial() def _initialize_environment(self): # wall to fridge: 4cm # fridge to goal: 1.5cm # hitman to range: 3.5cm # range to indigo: 3.5cm self.environment_poses = read_json(POSES_PATH) root_from_world = get_link_pose(self.kitchen, self.world_link) for name, world_from_part in self.environment_poses.items(): if name in ['range']: continue visual_path = os.path.join(KITCHEN_LEFT_PATH, '{}.obj'.format(name)) collision_path = os.path.join(KITCHEN_LEFT_PATH, '{}_collision.obj'.format(name)) mesh_path = None for path in [collision_path, visual_path]: if os.path.exists(path): mesh_path = path break if mesh_path is None: continue body = load_pybullet(mesh_path, fixed_base=True) root_from_part = multiply(root_from_world, world_from_part) if name in ['axe', 'dishwasher', 'echo', 'fox', 'golf']: (pos, quat) = root_from_part # TODO: still not totally aligned pos = np.array(pos) + np.array([0, -0.035, 0]) # , -0.005]) root_from_part = (pos, quat) self.environment_bodies[name] = body set_pose(body, root_from_part) # TODO: release bounding box or convex hull # TODO: static object nonconvex collisions if TABLE_NAME in self.environment_bodies: body = self.environment_bodies[TABLE_NAME] _, (w, l, _) = approximate_as_prism(body) _, _, z = get_point(body) new_pose = Pose(Point(TABLE_X + l / 2, -TABLE_Y, z), Euler(yaw=np.pi / 2)) set_pose(body, new_pose) def _initialize_ik(self, urdf_path): if not USE_TRACK_IK: self.ik_solver = None return from trac_ik_python.trac_ik import IK # killall -9 rosmaster base_link = get_link_name( self.robot, parent_link_from_joint(self.robot, self.arm_joints[0])) tip_link = get_link_name(self.robot, child_link_from_joint(self.arm_joints[-1])) # limit effort and velocities are required # solve_type: Speed, Distance, Manipulation1, Manipulation2 # TODO: fast solver and slow solver self.ik_solver = IK(base_link=str(base_link), tip_link=str(tip_link), timeout=0.01, epsilon=1e-5, solve_type="Speed", urdf_string=read(urdf_path)) if not CONSERVITIVE_LIMITS: return lower, upper = self.ik_solver.get_joint_limits() buffer = JOINT_LIMITS_BUFFER * np.ones(len(self.ik_solver.joint_names)) lower, upper = lower + buffer, upper - buffer lower[6] = -MAX_FRANKA_JOINT7 upper[6] = +MAX_FRANKA_JOINT7 self.ik_solver.set_joint_limits(lower, upper) def _update_initial(self): # TODO: store initial poses as well? self.initial_saver = WorldSaver() self.goal_bq = FConf(self.robot, self.base_joints) self.goal_aq = FConf(self.robot, self.arm_joints) if are_confs_close(self.goal_aq, self.carry_conf): self.goal_aq = self.carry_conf self.goal_gq = FConf(self.robot, self.gripper_joints) self.initial_confs = [self.goal_bq, self.goal_aq, self.goal_gq] set_all_static() def is_real(self): return (self.task is not None) and self.task.real @property def constants(self): return self.special_confs + self.gripper_confs + self.initial_confs ######################### @property def base_joints(self): return joints_from_names(self.robot, BASE_JOINTS) @property def arm_joints(self): #if self.robot_name == EVE: # return get_eve_arm_joints(self.robot, arm=DEFAULT_ARM) joint_names = ['panda_joint{}'.format(1 + i) for i in range(7)] #joint_names = self.robot_yaml['cspace'] return joints_from_names(self.robot, joint_names) @property def gripper_joints(self): #if self.robot_yaml is None: # return [] joint_names = ['panda_finger_joint{}'.format(1 + i) for i in range(2)] #joint_names = [joint_from_name(self.robot, rule['name']) # for rule in self.robot_yaml['cspace_to_urdf_rules']] return joints_from_names(self.robot, joint_names) @property def kitchen_joints(self): joint_names = get_joint_names(self.kitchen, get_movable_joints(self.kitchen)) #joint_names = self.kitchen_yaml['cspace'] #return joints_from_names(self.kitchen, joint_names) return joints_from_names(self.kitchen, filter(ALL_JOINTS.__contains__, joint_names)) @property def base_link(self): return child_link_from_joint(self.base_joints[-1]) @property def franka_link(self): return parent_link_from_joint(self.robot, self.arm_joints[0]) @property def gripper_link(self): return parent_link_from_joint(self.robot, self.gripper_joints[0]) @property def tool_link(self): return link_from_name(self.robot, get_tool_link(self.robot)) @property def world_link(self): # for kitchen return BASE_LINK @property def door_links(self): door_links = set() for joint in self.kitchen_joints: door_links.update(get_link_subtree(self.kitchen, joint)) return door_links @property def static_obstacles(self): # link=None is fine # TODO: decompose obstacles #return [(self.kitchen, frozenset(get_links(self.kitchen)) - self.door_links)] return {(self.kitchen, frozenset([link])) for link in set(get_links(self.kitchen)) - self.door_links} | \ {(body, None) for body in self.environment_bodies.values()} @property def movable(self): # movable base return set(self.body_from_name) # frozenset? @property def fixed(self): # fixed base return set(self.environment_bodies.values()) | {self.kitchen} @property def all_bodies(self): return self.movable | self.fixed | {self.robot} @property def default_conf(self): # if self.robot_name == EVE: # # Eve starts outside of joint limits # # Eve starts outside of joint limits # conf = [np.average(get_joint_limits(self.robot, joint)) for joint in self.arm_joints] # #conf = np.zeros(len(self.arm_joints)) # #conf[3] -= np.pi / 2 # return conf return DEFAULT_ARM_CONF #conf = np.array(self.robot_yaml['default_q']) ##conf[1] += np.pi / 4 ##conf[3] -= np.pi / 4 #return conf ######################### # TODO: could perform base motion planning without free joints def get_base_conf(self): return get_joint_positions(self.robot, self.base_joints) def set_base_conf(self, conf): set_joint_positions(self.robot, self.base_joints, conf) def get_base_aabb(self): franka_links = get_link_subtree(self.robot, self.franka_link) base_links = get_link_subtree(self.robot, self.base_link) return aabb_union( get_aabb(self.robot, link) for link in set(base_links) - set(franka_links)) def get_world_aabb(self): return aabb_union(get_aabb(body) for body in self.fixed) # self.all_bodies def _update_custom_limits(self, min_extent=0.0): #robot_extent = get_aabb_extent(get_aabb(self.robot)) # Scaling by 0.5 to prevent getting caught in corners #min_extent = 0.5 * min(robot_extent[:2]) * np.ones(2) / 2 world_aabb = self.get_world_aabb() full_lower, full_upper = world_aabb base_limits = (full_lower[:2] - min_extent, full_upper[:2] + min_extent) base_limits[1][0] = COMPUTER_X - min_extent # TODO: robot radius base_limits[0][1] += 0.1 base_limits[1][1] -= 0.1 for handle in self.base_limits_handles: remove_debug(handle) self.base_limits_handles = [] #self.base_limits_handles.extend(draw_aabb(world_aabb)) z = get_point(self.floor)[2] + 1e-2 if DEBUG: self.base_limits_handles.extend(draw_base_limits(base_limits, z=z)) self.custom_limits = custom_limits_from_base_limits( self.robot, base_limits) return self.custom_limits def solve_trac_ik(self, world_from_tool, nearby_tolerance=INF): assert self.ik_solver is not None init_lower, init_upper = self.ik_solver.get_joint_limits() base_link = link_from_name(self.robot, self.ik_solver.base_link) world_from_base = get_link_pose(self.robot, base_link) tip_link = link_from_name(self.robot, self.ik_solver.tip_link) tool_from_tip = multiply( invert(get_link_pose(self.robot, self.tool_link)), get_link_pose(self.robot, tip_link)) world_from_tip = multiply(world_from_tool, tool_from_tip) base_from_tip = multiply(invert(world_from_base), world_from_tip) joints = joints_from_names( self.robot, self.ik_solver.joint_names) # self.ik_solver.link_names seed_state = get_joint_positions(self.robot, joints) # seed_state = [0.0] * self.ik_solver.number_of_joints lower, upper = init_lower, init_upper if nearby_tolerance < INF: tolerance = nearby_tolerance * np.ones(len(joints)) lower = np.maximum(lower, seed_state - tolerance) upper = np.minimum(upper, seed_state + tolerance) self.ik_solver.set_joint_limits(lower, upper) (x, y, z), (rx, ry, rz, rw) = base_from_tip # TODO: can also adjust tolerances conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw) self.ik_solver.set_joint_limits(init_lower, init_upper) if conf is None: return conf # if nearby_tolerance < INF: # print(lower.round(3)) # print(upper.round(3)) # print(conf) # print(get_difference(seed_state, conf).round(3)) set_joint_positions(self.robot, joints, conf) return get_configuration(self.robot) def solve_pybullet_ik(self, world_from_tool, nearby_tolerance): start_time = time.time() # Most of the time is spent creating the robot # TODO: use the waypoint version that doesn't repeatedly create the robot current_conf = get_joint_positions(self.robot, self.arm_joints) full_conf = sub_inverse_kinematics( self.robot, self.arm_joints[0], self.tool_link, world_from_tool, custom_limits=self.custom_limits ) # , max_iterations=1) # , **kwargs) conf = get_joint_positions(self.robot, self.arm_joints) max_distance = get_distance(current_conf, conf, norm=INF) if nearby_tolerance < max_distance: return None print('Nearby) time: {:.3f} | distance: {:.5f}'.format( elapsed_time(start_time), max_distance)) return full_conf def solve_inverse_kinematics(self, world_from_tool, nearby_tolerance=INF, **kwargs): if self.ik_solver is not None: return self.solve_trac_ik(world_from_tool, **kwargs) #if nearby_tolerance != INF: # return self.solve_pybullet_ik(world_from_tool, nearby_tolerance=nearby_tolerance) current_conf = get_joint_positions(self.robot, self.arm_joints) start_time = time.time() if nearby_tolerance == INF: generator = ikfast_inverse_kinematics(self.robot, PANDA_INFO, self.tool_link, world_from_tool, max_attempts=10, use_halton=True) else: generator = closest_inverse_kinematics( self.robot, PANDA_INFO, self.tool_link, world_from_tool, max_time=0.01, max_distance=nearby_tolerance, use_halton=True) conf = next(generator, None) #conf = sample_tool_ik(self.robot, world_from_tool, max_attempts=100) if conf is None: return conf max_distance = get_distance(current_conf, conf, norm=INF) #print('Time: {:.3f} | distance: {:.5f} | max: {:.5f}'.format( # elapsed_time(start_time), max_distance, nearby_tolerance)) set_joint_positions(self.robot, self.arm_joints, conf) return get_configuration(self.robot) ######################### def set_initial_conf(self): set_joint_positions(self.robot, self.base_joints, [2.0, 0, np.pi]) #for rule in self.robot_yaml['cspace_to_urdf_rules']: # gripper: max is open # joint = joint_from_name(self.robot, rule['name']) # set_joint_position(self.robot, joint, rule['value']) set_joint_positions(self.robot, self.arm_joints, self.default_conf) # active_task_spaces # if self.robot_name == EVE: # for arm in ARMS: # joints = get_eve_arm_joints(self.robot, arm)[2:4] # set_joint_positions(self.robot, joints, -0.2*np.ones(len(joints))) def set_gripper(self, value): positions = value * np.ones(len(self.gripper_joints)) set_joint_positions(self.robot, self.gripper_joints, positions) def close_gripper(self): self.closed_gq.assign() def open_gripper(self): self.open_gq.assign() ######################### def get_door_sign(self, joint): return -1 if 'left' in get_joint_name(self.kitchen, joint) else +1 def closed_conf(self, joint): lower, upper = get_joint_limits(self.kitchen, joint) if 'drawer' in get_joint_name(self.kitchen, joint): fraction = 0.9 return fraction * lower + (1 - fraction) * upper if 'left' in get_joint_name(self.kitchen, joint): return upper return lower def open_conf(self, joint): joint_name = get_joint_name(self.kitchen, joint) if 'left' in joint_name: open_position = get_min_limit(self.kitchen, joint) else: open_position = get_max_limit(self.kitchen, joint) #print(get_joint_name(self.kitchen, joint), get_min_limit(self.kitchen, joint), get_max_limit(self.kitchen, joint)) # drawers: [0.0, 0.4] # left doors: [-1.57, 0.0] # right doors: [0.0, 1.57] if joint_name in CABINET_JOINTS: # TODO: could make fraction of max return CABINET_OPEN_ANGLE * open_position / abs(open_position) if joint_name in DRAWER_JOINTS: return DRAWER_OPEN_FRACTION * open_position return open_position def close_door(self, joint): set_joint_position(self.kitchen, joint, self.closed_conf(joint)) def open_door(self, joint): set_joint_position(self.kitchen, joint, self.open_conf(joint)) ######################### def add_camera(self, name, pose, camera_matrix, max_depth=KINECT_DEPTH, display=False): color = apply_alpha(RED, 0.1 if DEBUG else 0) cone = get_viewcone(depth=max_depth, camera_matrix=camera_matrix, color=color, mass=0, collision=False) set_pose(cone, pose) if display: kinect = load_pybullet(KINECT_URDF, fixed_base=True) set_pose(kinect, pose) set_color(kinect, BLACK) self.add(name, kinect) self.cameras[name] = Camera(cone, camera_matrix, max_depth) if DEBUG: draw_pose(pose) step_simulation() return name def get_supporting(self, obj_name): # is_placed_on_aabb | is_center_on_aabb # Only want to generate stable placements, but can operate on initially unstable ones # TODO: could filter orientation as well body = self.get_body(obj_name) supporting = { surface for surface in ALL_SURFACES if is_center_on_aabb(body, compute_surface_aabb(self, surface), above_epsilon=5e-2, below_epsilon=5e-2) } if ('range' in supporting) and (len(supporting) == 2): # TODO: small hack for now supporting -= {'range'} if len(supporting) != 1: print('{} is not supported by a single surface ({})!'.format( obj_name, supporting)) return None [surface_name] = supporting return surface_name def fix_pose(self, name, pose=None, fraction=0.5): body = self.get_body(name) if pose is None: pose = get_pose(body) else: set_pose(body, pose) # TODO: can also drop in simulation x, y, z = point_from_pose(pose) roll, pitch, yaw = euler_from_quat(quat_from_pose(pose)) quat = quat_from_euler(Euler(yaw=yaw)) set_quat(body, quat) surface_name = self.get_supporting(name) if surface_name is None: return None, None if fraction == 0: new_pose = (Point(x, y, z), quat) return new_pose, surface_name surface_aabb = compute_surface_aabb(self, surface_name) new_z = (1 - fraction) * z + fraction * stable_z_on_aabb( body, surface_aabb) point = Point(x, y, new_z) set_point(body, point) print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format( name, roll, pitch, new_z - z)) new_pose = (point, quat) return new_pose, surface_name # def fix_geometry(self): # for name in self.movable: # fixed_pose, _ = self.fix_pose(name) # if fixed_pose is not None: # set_pose(self.get_body(name), fixed_pose) ######################### def add(self, name, body): assert name not in self.body_from_name if DEBUG: add_body_name(body, name) self.body_from_name[name] = body return name def add_body(self, name, **kwargs): obj_type = type_from_name(name) self.names_from_type.setdefault(obj_type, []).append(name) path = get_obj_path(obj_type) #self.path_from_name[name] = path print('Loading', path) body = load_pybullet(path, **kwargs) assert body is not None self.add(name, body) def get_body(self, name): return self.body_from_name[name] # def get_body_path(self, name): # return self.path_from_name[name] # def get_body_type(self, name): # filename, _ = os.path.splitext(os.path.basename(self.get_body_path(name))) # return filename def get_name(self, name): inverse = {v: k for k, v in self.body_from_name.items()} return inverse.get(name, None) def remove_body(self, name): body = self.get_body(name) remove_body(body) del self.body_from_name[name] def reset(self): #remove_all_debug() for camera in self.cameras.values(): remove_body(camera.body) #remove_body(camera.kinect) self.cameras = {} for name in list(self.body_from_name): self.remove_body(name) def destroy(self): reset_simulation() disconnect()