def __init__(self, basename, simulation=True): self.simulation=simulation if self.simulation: self.port=new_port(basename+"/xo:in", "in", "/slider_sim/xo:out", timeout=10.) else: self.port=new_port(basename+"/xo:in", "in", "/marker/object_pos:o", timeout=10.) pass #TODO: create port for marker tracking in case of using cop
def __init__(self,arm_portbasename, handlername="/HandlerArm"): import imp prename=arm_portbasename full_name=prename+ handlername self.outp=yarp.BufferedPortBottle() outp_name=full_name +"/toObjectFeeder" self.outp.open(outp_name) self.stiffness_port=yarp.BufferedPortBottle() stiffness_port_name=full_name +"/stiffness" self.stiffness_port.open(stiffness_port_name) self.goaldistp=yarp.BufferedPortBottle() goaldistp_name=full_name + "/fromGoalDistance" self.goaldistp.open(goaldistp_name) #self.goaldistp.setStrict() self.posep=yarp.BufferedPortBottle() posep_name=full_name+"/pose:i" self.posep.open(posep_name) yarp_connect_blocking(outp_name,prename+"/ofeeder/object") yarp_connect_blocking(stiffness_port_name,prename+"/robot/stiffness") yarp_connect_blocking(prename+"/dmonitor/distOut" , goaldistp_name) yarp_connect_blocking(prename+"/vectorField/pose", posep_name) self.current_frame=[1.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0] self.current_slowdown_distance=0.1 self.toolp = new_port(full_name + "/toToolin", 'out', "%s/vectorField/tool"%(prename)) self.toolp.setStrict(True) self.goal_threshold=0.01
def set_params(self, params): # General initialization, takes the dictionary params and # puts everything in variables of the class vars(self).update(params) self.raw_key = Raw_key() base_name = "/goal_obstacle" # goal port and position initialization self.goal_out_port_list = [] self.goal_pose_list = [0] * self.robot_cant self.goal_vel_list = [0] * self.robot_cant for i in range(self.robot_cant): self.goal_out_port_list.append( new_port( base_name + "/goal" + i.__str__() + ":out", "out", "/differential_robot/goal" + i.__str__() + ":in", timeout=2)) self.goal_pose_list[i] = identity(4) self.goal_pose_list[i][:3, 3] = array([0.1, i, 0.]) self.goal_vel_list[i] = array([0., 0., 0., 0., 0., 0.]) # obstacle port position initialization self.obstacle_out_port_list = [] self.obstacle_pose_list = [0] * self.obstacle_cant self.obstacle_vel_list = [0] * self.obstacle_cant for i in range(self.obstacle_cant): self.obstacle_out_port_list.append( new_port( base_name + "/obstacle" + i.__str__() + ":out", "out", "/differential_robot/obstacle" + i.__str__() + ":in", timeout=2)) self.obstacle_pose_list[i] = identity(4) self.obstacle_vel_list[i] = array([0., 0., 0., 0., 0., 0.]) # operation information self.random_movement = False self.robot_index = 0 self.obstacle_index = 0 self.last_time = time.time() self.inc_vel = 0.43
def __init__(self, baseportname, remote_name): self.port=new_port(baseportname+"/force:i", "in", remote_name, timeout=10.0) self.update_data(blocking=True)
def set_params(self, params): # General initialization takes the dictionary params and puts # everything in variables of the class vars(self).update( params ) base_name = "/differential_robot" # port initialization self.goal_in_port_list = [] for i in range(self.robot_cant): self.goal_in_port_list.append( new_port( base_name + "/goal" + i.__str__() + ":in", "in", "/goal_obstacle/goal" + i.__str__() + ":out", timeout=1.)) self.obstacle_in_port_list = [] for i in range(self.obstacle_cant): self.obstacle_in_port_list.append( new_port( base_name + "/obstacle" + i.__str__() + ":in", "in", "/goal_obstacle/obstacle" + i.__str__() + ":out", timeout=1.)) self.last_time = time.time() # differential_robots self.robot_list = [] # variables for each robot self.robot_pose_list = [] self.robot_vel_twist_list = [] self.robot_pose_out_list = [] self.phi_l_list = [] self.phi_r_list = [] self.angle_list = [] self.omega_list = [] self.required_velocity_list = [] self.goal_pose_list = [] robot_id_offset = identity(4) for i in range(self.robot_cant): # initialization if self.robot_type_list[i] == "differential": self.robot_list.append(Differential_robot( )) # this line defines what robot is going to be used else: self.robot_list.append(Differential_robot( )) # this line defines what robot is going to be used self.robot_pose_list.append(identity(4)) self.robot_pose_list[i][:3, 3] = array([1.85, i, 0.]) self.robot_vel_twist_list.append(array([0., 0., 0., 0., 0., 0.])) self.robot_pose_out_list.append(array(self.robot_pose_list[i])) self.phi_l_list.append(0.0) self.phi_r_list.append(0.0) self.angle_list.append(0.0) self.omega_list.append(0.0) self.required_velocity_list.append(array([0., 0., 0.])) self.goal_pose_list.append(identity(4)) # visualization self.view_objects.send_prop(self.robot_id_list[i], "scale", self.robot_list[i].robot_dim) self.view_objects.send_prop(self.robot_id_list[i], "pose_offset", robot_id_offset.reshape(16).tolist()) self.raw_key = Raw_key() # choose a set of equations self.move = False self.goal_constant = 1. self.obstacle_constant = 1. self.damp_constant = 0.5 self.obstacle_pose_list = [1] * self.obstacle_cant for i in range(self.obstacle_cant): self.obstacle_pose_list[i] = identity(4)