def get_object_global_bbox(self, object_name): """ Returns the global bounding box of an object as list encapsulated as string: "[[x0, y0, z0 ], ... ,[x7, y7, z7]]". :param string object_name: The name of the object. """ # Test whether the object exists in the scene b_obj = get_obj_by_name(object_name) # Get bounding box of object bb = blenderapi.objectdata(object_name).bound_box # Group x,y,z-coordinates as lists bbox_local = [[bb_corner[i] for i in range(3)] for bb_corner in bb] world_pos = b_obj.worldPosition world_ori = b_obj.worldOrientation.to_3x3() bbox_global = [] for corner in bbox_local: vec = world_ori * mathutils.Vector(corner) + \ mathutils.Vector(world_pos) bbox_global.append([vec.x, vec.y, vec.z]) return json.dumps(bbox_global)
def __init__(self, obj, parent=None): """ Constructor method. Receives the reference to the Blender object. The second parameter should be the name of the object's parent. """ logger.info('%s initialization' % obj.name) # Call the constructor of the parent class super(self.__class__, self).__init__(obj, parent) # The robot needs a physics controller! # Since the imu does not have physics, self.has_physics = bool(self.robot_parent.bge_object.getPhysicsId()) if not self.has_physics: logger.warning("The robot doesn't have a physics controller," \ "falling back to simple IMU sensor.") if self.has_physics: # make new references to the robot velocities and use those. self.robot_w = self.robot_parent.bge_object.localAngularVelocity self.robot_vel = self.robot_parent.bge_object.worldLinearVelocity else: # reference to sensor position self.pos = self.bge_object.worldPosition # previous position self.pp = self.pos.copy() # previous attitude euler angles as vector self.patt = mathutils.Vector(self.position_3d.euler) # previous linear velocity self.plv = mathutils.Vector((0.0, 0.0, 0.0)) # previous angular velocity self.pav = mathutils.Vector((0.0, 0.0, 0.0)) # get gravity from scene? #g = bpy.data.scenes[0].game_settings.physics_gravity g = 9.81 self.gravity = mathutils.Vector((0.0, 0.0, g)) # imu2body will transform a vector from imu frame to body frame self.imu2body = self.sensor_to_robot_position_3d() # rotate vector from body to imu frame self.rot_b2i = self.imu2body.rotation.conjugated() logger.debug("imu2body rotation RPY [% .3f % .3f % .3f]" % tuple(math.degrees(a) for a in self.imu2body.euler)) logger.debug("imu2body translation [% .3f % .3f % .3f]" % tuple(self.imu2body.translation)) if (self.imu2body.translation.length > 0.01): self.compute_offset_acceleration = True else: self.compute_offset_acceleration = False # reference for rotating a vector from imu frame to world frame self.rot_i2w = self.bge_object.worldOrientation logger.info("IMU Component initialized, runs at %.2f Hz ", self.frequency)
def __init__(self, obj, parent=None): """ Constructor method. Receives the reference to the Blender object. The second parameter should be the name of the object's parent. """ logger.info('%s initialization' % obj.name) # Call the constructor of the parent class morse.core.sensor.Sensor.__init__(self, obj, parent) has_physics = bool(self.robot_parent.bge_object.getPhysicsId()) if self._type == 'Automatic': if has_physics: self._type = 'Velocity' else: self._type = 'Position' if self._type == 'Velocity' and not has_physics: logger.error( "Invalid configuration : Velocity computation without " "physics") return if self._type == 'Velocity': # make new references to the robot velocities and use those. self.robot_w = self.robot_parent.bge_object.localAngularVelocity self.robot_vel = self.robot_parent.bge_object.worldLinearVelocity else: self.pp = copy(self.position_3d) # previous linear velocity self.plv = mathutils.Vector((0.0, 0.0, 0.0)) # previous angular velocity self.pav = mathutils.Vector((0.0, 0.0, 0.0)) self.gravity = -blenderapi.gravity() # imu2body will transform a vector from imu frame to body frame self.imu2body = self.sensor_to_robot_position_3d() # rotate vector from body to imu frame self.rot_b2i = self.imu2body.rotation.conjugated() logger.debug("imu2body rotation RPY [% .3f % .3f % .3f]" % tuple(math.degrees(a) for a in self.imu2body.euler)) logger.debug("imu2body translation [% .3f % .3f % .3f]" % tuple(self.imu2body.translation)) if self.imu2body.translation.length > 0.01: self.compute_offset_acceleration = True else: self.compute_offset_acceleration = False # reference for rotating a vector from imu frame to world frame self.rot_i2w = self.bge_object.worldOrientation self.mag = MagnetoDriver() logger.info("IMU Component initialized, runs at %.2f Hz ", self.frequency)
def _store_joint_position(self, joints, ik_target, joint_index): joint_position = joints[joint_index].position # Convert the GEN_POINT_3D into a Blender vector position_vector = mathutils.Vector([joint_position.x, joint_position.y, joint_position.z]) if transformation_matrix: #logger.error("Kinect position: [%.4f, %.4f, %.4f]" % (transformation_matrix[0][3], transformation_matrix[1][3], transformation_matrix[2][3])) new_position = position_vector * transformation_matrix new_position = new_position + mathutils.Vector([transformation_matrix[0][3], transformation_matrix[1][3], transformation_matrix[2][3]]) else: new_position = position_vector self.data[ik_target] = new_position
def get_wheels(self): # get pointers to and physicsIds of all objects # get wheel pointers - needed by wheel speed sensors and to # set up constraints # bullet vehicles always have 4 wheels scene = blenderapi.scene() self._wheel_radius = None caster_wheel_name = self.bge_object.get('CasterWheelName', None) # inherited from the parent robot for index in self._wheel_index: name = "Wheel%sName" % index # Get the actual name of the object from the properties # of the parent robot try: wheel = scene.objects[self.bge_object[name]] except: #import traceback #traceback._exc() wheel = None if wheel: self._wheels[index] = wheel logger.info("\tWheel %s: %s" % (index, wheel.name)) self._wheel_positions[index] = \ mathutils.Vector(wheel.worldPosition) self._wheel_orientations[index] = \ mathutils.Matrix(wheel.worldOrientation) # Make the wheels orphans wheel.removeParent() # Keep their transformations #wheel.worldPosition = self._wheel_positions[index] #wheel.worldOrientation = self._wheel_orientations[index] # get wheel radius if not already computed if wheel.name != caster_wheel_name and not self._wheel_radius: self._wheel_radius = self.get_wheel_radius( self.bge_object[name]) logger.debug("get_wheels %s" % self._wheels) # Add a free rotating wheel if indicated in the robot if caster_wheel_name and caster_wheel_name != 'None': wheel = scene.objects[caster_wheel_name] wheel_position = mathutils.Vector(wheel.worldPosition) self.attach_caster_wheel_to_body(wheel, self.bge_object, wheel_position)
def apply_speed(self, kind, linear_speed, angular_speed): """ Apply speed parameter to the robot :param string kind: the kind of control to apply. Can be one of ['Position', 'Velocity']. :param list linear_speed: the list of linear speed to apply, for each axis, in m/s. :param list angular_speed: the list of angular speed to apply, for each axis, in rad/s. """ parent = self.bge_object must_fight_against_gravity = self.is_dynamic and self._no_gravity if must_fight_against_gravity: parent.applyForce(-blenderapi.gravity()) if kind == 'Position': if must_fight_against_gravity: parent.worldLinearVelocity = [0.0, 0.0, 0.0] parent.applyMovement(linear_speed, True) parent.applyRotation(angular_speed, True) elif kind == 'Velocity': if self._is_ground_robot: """ A ground robot cannot control its vz not rx, ry speed in the world frame. So convert {linear, angular}_speed in world frame, remove uncontrolable part and then pass it against in the robot frame""" linear_speed = mathutils.Vector(linear_speed) angular_speed = mathutils.Vector(angular_speed) linear_speed.rotate(parent.worldOrientation) angular_speed.rotate(parent.worldOrientation) linear_speed[2] = parent.worldLinearVelocity[2] angular_speed[0] = parent.worldAngularVelocity[0] angular_speed[1] = parent.worldAngularVelocity[1] linear_speed.rotate(parent.worldOrientation.transposed()) angular_speed.rotate(parent.worldOrientation.transposed()) # Workaround against 'strange behaviour' for robot with # 'Dynamic' Physics Controller. [0.0, 0.0, 0.0] seems to be # considered in a special way, i.e. is basically ignored. # Setting it to 1e-6 instead of 0.0 seems to do the trick! if angular_speed == [0.0, 0.0, 0.0]: angular_speed = [0.0, 0.0, 1e-6] parent.setLinearVelocity(linear_speed, True) parent.setAngularVelocity(angular_speed, True)
def sim_imu_simple(self): """ Simulate angular velocity and linear acceleration measurements via simple differences. """ # Compute the differences with the previous loop #dp = self.pos - self.pp #deuler = mathutils.Vector(self.position_3d.euler - self.peuler) # linear and angular velocities lin_vel = (self.pos - self.pp) * self.frequency att = mathutils.Vector(self.position_3d.euler) ang_vel = (att - self.patt) * self.frequency # linear acceleration in imu frame dv_imu = self.rot_w2i.transposed() * (lin_vel - self.plv) * self.frequency # measurement includes gravity and acceleration accel_meas = dv_imu + self.rot_w2i.transposed() * self.gravity # save current position and attitude for next step self.pp = self.pos.copy() self.peuler = att # save velocity for next step self.plv = lin_vel self.pav = ang_vel return (ang_vel, accel_meas)
def __init__(self, obj): self.name = obj.name self.type = obj['Type'] self.obj = obj mesh = obj.meshes[0] # Get list of unique vertexes vertexes_ = set() for v_index in range(24): vertex = mesh.getVertex(0, v_index) vertexes_.add(vertex.getXYZ()[:]) vertexes = [mathutils.Vector(p) for p in vertexes_] # Compute range self._min_values = [float_info.max, float_info.max, float_info.max] self._max_values = [-float_info.max, -float_info.max, -float_info.max] for i in range(len(vertexes)): for j in range(3): # XXX Here, we assume there is no rotation vertexes[i][j] = vertexes[i][j] * obj.worldScale[j] + obj.worldPosition[j] if self._min_values[j] > vertexes[i][j]: self._min_values[j] = vertexes[i][j] if self._max_values[j] < vertexes[i][j]: self._max_values[j] = vertexes[i][j]
def transform_to_obj_frame(self, object_name, point): """ Transforms a 3D point with respect to the origin into the coordinate frame of an object and returns the global coordinates. :param string object_name: The name of the object. :param string point: coordinates as a list "[x, y, z]" """ # Test whether the object exists in the scene b_obj = get_obj_by_name(object_name) world_pos = b_obj.worldPosition world_ori = b_obj.worldOrientation.to_3x3() pos = world_ori * mathutils.Vector(json.loads(point)) + \ mathutils.Vector(world_pos) return [pos.x, pos.y, pos.z]
def compute(self, pose): pos = numpy.matrix(pose.translation) pos_lla = self._coord_conv.ltp_to_geodetic(pos) (decl, incl, f, h, x, y, z) = self._mag.compute(degrees(pos_lla[0, 0]), degrees(pos_lla[0, 1]), pos_lla[0, 2] / 1000.0, self._date) mag_field = mathutils.Vector((x, y, z)) return mag_field * pose.rotation_matrix
def force_pose(self, position, orientation): me = self.aruco['aruco'] parent = self.aruco['parent'] pose3d = parent.position_3d parent_pose = mathutils.Vector((pose3d.x, pose3d.y, pose3d.z)) if position: me.worldPosition = position + parent_pose if orientation: me.worldOrientation = orientation
def set_gravity(self, gravity=9.81): """ Set the gravity for the specific scene :param gravity: float, default: 9.81 """ if isinstance(gravity, float): bpymorse.get_context_scene( ).game_settings.physics_gravity = gravity bpymorse.get_context_scene().gravity = mathutils.Vector( (0.0, 0.0, -gravity))
def sim_attitude_simple(self): """ Simulate angular velocity measurements via simple differences. """ # linear and angular velocities att = mathutils.Vector(self.position_3d.euler) ang_vel = (att - self.patt) * self.frequency self.patt = att return ang_vel
def default_action(self): """ Apply (force, torque) to the parent robot. """ # Get the the parent robot robot = self.robot_parent if self._robot_frame: # directly apply local forces and torques to the blender object of the parent robot robot.bge_object.applyForce(self.local_data['force'], True) robot.bge_object.applyTorque(self.local_data['torque'], True) else: (loc, rot, scale) = robot.position_3d.transformation3d_with( self.position_3d).matrix.decompose() # rotate into robot frame, but still at actuator origin force = rot * mathutils.Vector(self.local_data['force']) torque = rot * mathutils.Vector(self.local_data['torque']) # add torque due to lever arm torque += loc.cross(force) robot.bge_object.applyForce(force, True) robot.bge_object.applyTorque(torque, True)
def gravity(): if not fake: sce = scene() # All supported version of blender do not support it, so well # guess if we don't have the support if hasattr(sce, 'gravity'): return sce.gravity else: return mathutils.Vector((0.0, 0.0, -9.81)) else: return None
def default_action(self): position = mathutils.Vector( (self.local_data['z']+self._xoffset, self.local_data['x']+self._yoffset, (-1.0*self.local_data['y']+self._zoffset)) ) orientation = mathutils.Euler([ self.local_data['roll' ], self.local_data['pitch'], self.local_data['yaw' ] ]) """ Convert Euler to Matrix, worldOrientation accepts Quat, Mat """ orientation.order = "YZX" orientation_mat = orientation.to_matrix() self.force_pose(position, orientation_mat)
def default_action(self): ''' Change the parent robot pose. ''' # New parent position position = mathutils.Vector( (self.local_data['x'], self.local_data['y'], self.local_data['z'])) # New parent orientation phi = self.local_data['phi'] theta = self.local_data['theta'] psi = self.local_data['psi'] rotation_matrix = self.get_taitbryan_xyz(phi, theta, psi) self.robot_parent.force_pose(position, rotation_matrix)
def default_action(self): """ Change the parent robot pose. """ # New parent position position = mathutils.Vector( (self.local_data['x'], self.local_data['y'], self.local_data['z'])) # New parent orientation orientation = mathutils.Euler([ self.local_data['roll'], self.local_data['pitch'], self.local_data['yaw'] ]) self.robot_parent.force_pose(position, orientation.to_matrix())
def default_action(self): """ Apply ... to the parent robot. """ # Ticks dt = 1.0 / self.frequency # Compute height and Euler Angles self.f_phi.simulate(self.local_data['phi_c'], dt) self.f_theta.simulate(self.local_data['theta_c'], dt) self.f_psi.simulate(self.f_psi.x[0]-self.local_data['psi_c'], dt) self.f_h.simulate(self.local_data['h_c'], dt) rot = mathutils.Euler([self.f_phi.x[0], self.f_theta.x[0], self.f_psi.x[0]]) # Get the parent parent = self.robot_parent.bge_object # Compute acceleration # get previous height and vert. velocity main_to_origin = self.robot_parent.position_3d main_to_origin.update(parent) h = main_to_origin.z # assume velocity acc_b = mathutils.Vector([0.0, 0.0, 10.0 + 9.0 * (self.local_data['h_c'] - h) - 2.0 * 0.7 * 3.0 * self.v[2]]) acc_i = mathutils.Vector( rot.to_matrix()*acc_b ) self.v[0] += dt * acc_i[0] self.v[1] += dt * acc_i[1] self.v[2] += dt * (acc_i[2] - 10.0) #Change the parent position prev_pos = parent.localPosition parent.localPosition = mathutils.Vector(prev_pos + dt * mathutils.Vector([self.v[0],self.v[1],self.v[2]])) #Change the parent orientation parent.orientation = rot.to_matrix()
def angular_velocities(prev, now, dt): """ Return angular velocities between two poses :param prev: the precedent pose, as a Transformation3d :param now: the current pose, as a Transformation3d :param dt: time elapsed between the two poses acquisition, in sec See https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf for equation description """ patt = mathutils.Vector(prev.euler) att = mathutils.Vector(now.euler) euler_rate = (att - patt) / dt c0 = cos(att[0]) c1 = cos(att[1]) s0 = sin(att[0]) s1 = sin(att[1]) m = mathutils.Matrix(([1, 0, -s1], [0, c0, c1 * s0], [0, -s0, c1 * c0])) return m * euler_rate
def default_action(self): """ Change the parent robot pose. """ # New parent position position = mathutils.Vector( (self.local_data['x'], self.local_data['y'], self.local_data['z'])) # New parent orientation orientation = mathutils.Euler([ self.local_data['roll'], self.local_data['pitch'], self.local_data['yaw'] ]) world2actuator = Transformation3d(None) world2actuator.translation = position world2actuator.rotation = orientation (loc, rot, _) = (world2actuator.matrix * self.actuator2robot.matrix).decompose() self.robot_parent.force_pose(loc, rot)
def default_action(self): """ Change the parent robot pose. """ # Get the Blender object of the parent robot parent = self.robot_parent.bge_object # New parent position position = mathutils.Vector((self.local_data['x'], \ self.local_data['y'], \ self.local_data['z'])) # New parent orientation orientation = mathutils.Euler([self.local_data['roll'], \ self.local_data['pitch'], \ self.local_data['yaw']]) # Suspend Bullet physics engine, which doesnt like teleport # or instant rotation done by the Orientation actuator (avoid tilt) parent.suspendDynamics() parent.worldPosition = position parent.worldOrientation = orientation.to_matrix() parent.restoreDynamics()
def convert_LTP_to_ECEF(P): """ converts point in LTP(Blender) to ECEF-r coordinates """ x0 = convert_GPS_to_ECEF(P) #P->x0 x0 = mathutils.Vector(x0) transform_matrix = [[-math.sin(P[0]), math.cos(P[0]), 0], [ -math.cos(P[0]) * math.sin(P[1]), -math.sin(P[1]) * math.sin(P[0]), math.cos(P[1]) ], [ math.cos(P[1]) * math.cos(P[0]), math.cos(P[1]) * math.sin(P[0]), math.sin(P[1]) ]] transform_matrix = mathutils.Matrix(transform_matrix) transform_matrix.invert() xe = x0 + transform_matrix * xt #transformed xt -> xe return xe
def __init__(self, obj, parent=None): """ Constructor method. Receives the reference to the Blender object. The second parameter should be the name of the object's parent. """ logger.info('%s initialization' % obj.name) # Call the constructor of the parent class morse.core.sensor.Sensor.__init__(self, obj, parent) has_physics = bool(self.robot_parent.bge_object.getPhysicsId()) if self._type == 'Automatic': if has_physics: self._type = 'Velocity' else: self._type = 'Position' if self._type == 'Velocity' and not has_physics: logger.error( "Invalid configuration : Velocity computation without " "physics") return if self._type == 'Velocity': # make new references to the robot velocities and use those. self.robot_w = self.robot_parent.bge_object.localAngularVelocity else: # previous attitude euler angles as vector self.patt = mathutils.Vector(self.position_3d.euler) # previous angular velocity # imu2body will transform a vector from imu frame to body frame self.imu2body = self.sensor_to_robot_position_3d() # rotate vector from body to imu frame self.rot_b2i = self.imu2body.rotation.conjugated() logger.info("Attitude Component initialized, runs at %.2f Hz ", self.frequency)
def set_object_pose(self, object_name, position, orientation): """ Sets the pose of the object. :param string object_name: The name of the object. :param string position: new position of the object [x, y, z] :param string position: new orientation of the object [qw, qx, qy, qz] """ b_obj = get_obj_by_name(object_name) pos = mathutils.Vector(json.loads(position)) ori = mathutils.Quaternion(json.loads(orientation)).to_matrix() # Suspend Physics of that object b_obj.suspendDynamics() b_obj.setLinearVelocity([0.0, 0.0, 0.0], True) b_obj.setAngularVelocity([0.0, 0.0, 0.0], True) b_obj.applyForce([0.0, 0.0, 0.0], True) b_obj.applyTorque([0.0, 0.0, 0.0], True) logger.debug("%s goes to %s" % (b_obj, pos)) b_obj.worldPosition = pos b_obj.worldOrientation = ori # Reset physics simulation b_obj.restoreDynamics()
def default_action(self): """ Calculates speed and GPS-position Configurations are the GPS-values for the Blenderorigin Transforms point from LTP to Geodetic coordinates Refer to: - Conversion of Geodetic coordinates to the Local Tangent Plane, Version 2.01, http://psas.pdx.edu/CoordinateSystem/Latitude_to_LocalTangent.pdf - Comparative Analysis of the Performance of Iterative and Non-iterative Solutions to the Cartesian to Geodetic Coordinate Transformation, Hok Sum Fok and H. Bâki Iz, http://www.lsgi.polyu.edu.hk/staff/zl.li/Vol_5_2/09-baki-3.pdf """ #### #Speed #### ##copied from accelerometer # Compute the difference in positions with the previous loop self.dx = self.p[0] - self.ppx self.dy = self.p[1] - self.ppy self.dz = self.p[2] - self.ppz # Store the position in this instant self.ppx = self.p[0] self.ppy = self.p[1] self.ppz = self.p[2] # Scale the speeds to the time used by Blender self.v[0] = self.dx * self.frequency self.v[1] = self.dy * self.frequency self.v[2] = self.dz * self.frequency # Update the data for the velocity self.pvx = self.v[0] self.pvy = self.v[1] self.pvz = self.v[2] #### #GPS #### #### #constants in calculations #a: WGS-84 Earth semimajor axis #ecc: first eccentricity #### a = float(6378137) ecc = 8.181919191e-2 def convert_GPS_to_ECEF(P): """ converts gps-data(radians) to ECEF-r coordinates """ N = a / math.sqrt(1 - (ecc**2 * (math.sin(P[1])**2))) h = P[2] x0 = [(h + N) * math.cos(P[1]) * math.cos(P[0]), (h + N) * math.cos(P[1]) * math.sin(P[0]), (h + (1 - ecc**2) * N) * math.sin(P[1])] return x0 def convert_LTP_to_ECEF(P): """ converts point in LTP(Blender) to ECEF-r coordinates """ x0 = convert_GPS_to_ECEF(P) #P->x0 x0 = mathutils.Vector(x0) transform_matrix = [[-math.sin(P[0]), math.cos(P[0]), 0], [ -math.cos(P[0]) * math.sin(P[1]), -math.sin(P[1]) * math.sin(P[0]), math.cos(P[1]) ], [ math.cos(P[1]) * math.cos(P[0]), math.cos(P[1]) * math.sin(P[0]), math.sin(P[1]) ]] transform_matrix = mathutils.Matrix(transform_matrix) transform_matrix.invert() xe = x0 + transform_matrix * xt #transformed xt -> xe return xe def vermeille_method(xe): """ converts point in ECEF-r coordinates into Geodetic (GPS) via Vermeille's method """ #"just intermediary parameters" see FoIz p = (xe[0]**2 + xe[1]**2) / a**2 q = (1 - ecc**2) / a**2 * xe[2]**2 r = (p + q - ecc**4) / 6 s = ecc**4 * (p * q) / (4 * r**3) t = (1 + s + math.sqrt(s * (2 + s)))**(1 / 3.0) u = r * (1 + t + 1 / t) v = math.sqrt(u**2 + (ecc**4 * q)) w = ecc**2 * ((u + v - q) / (2 * v)) k = math.sqrt(u + v + w**2) - w D = (k * (math.sqrt(xe[0]**2 + xe[1]**2))) / (k + ecc**2) gps_coords = [ 2 * math.atan(xe[1] / (xe[0] + (math.sqrt(xe[0]**2 + xe[1]**2)))), 2 * math.atan(xe[2] / (D + math.sqrt(D**2 + xe[2]**2))), ((k + ecc**2 - 1) / k) * math.sqrt(D**2 + xe[2]**2) ] return gps_coords #P -> Blender origin in Geodetic coordinates P = [self.longitude, self.latitude, self.altitude] #current position xt = self.position_3d.translation xt = mathutils.Vector(xt) #P (in degrees) to radians for i in range(len(P) - 1): P[i] = math.radians(P[i]) #### #GPS -> ECEF-r #### xe = convert_LTP_to_ECEF(P) #### #ECEF-r -> GPS #### gps_coords = vermeille_method(xe) #gps_coords (in radians) to degrees for i in range(len(gps_coords) - 1): gps_coords[i] = math.degrees(gps_coords[i]) #compose message as close as possible to a GPS-standardprotocol self.local_data['longitude'] = gps_coords[0] self.local_data['latitude'] = gps_coords[1] self.local_data['altitude'] = gps_coords[2] self.local_data['velocity'] = self.v
def default_action(self): """ Apply the positions read to the IK targets of the joints """ # Compute a rotation angle for the whole body, based on the # angle of the shoulders world_x_vector = mathutils.Vector([1, 0, 0]) shoulders_vector = mathutils.Vector([ (self._shoulder_empty_l.worldPosition[0] - self._shoulder_empty_r.worldPosition[0]), (self._shoulder_empty_l.worldPosition[1] - self._shoulder_empty_r.worldPosition[1]), (self._shoulder_empty_l.worldPosition[2] - self._shoulder_empty_r.worldPosition[2]) ]) logger.debug ("Shoulder Vector: [%.4f, %.4f, %.4f]" % \ (shoulders_vector[0], shoulders_vector[1], shoulders_vector[2])) try: # Measure the angle with respect to the X axis (in front of the man) body_rotation = shoulders_vector.angle(world_x_vector) # Correct the angle body_rotation -= pi / 2 except ValueError: # There will be an error if there is no user being tracked # by the Kinect body_rotation = 0.0 logger.debug("Angle with Y vector = %.4f" % body_rotation) # Apply the rotation to the toroso. The rest of the body should follow self._torso_empty.worldOrientation = [0.0, 0.0, body_rotation] # Put the ik targets in the correct positions self._set_object_position(self._head_empty.worldPosition, self.local_data['head_position']) self._set_object_position(self._neck_empty.worldPosition, self.local_data['neck_position']) self._set_object_position(self._torso_empty.worldPosition, self.local_data['torso_position']) self._set_object_position(self._hand_empty_l.worldPosition, self.local_data['left_hand_position']) self._set_object_position(self._hand_empty_r.worldPosition, self.local_data['right_hand_position']) self._set_object_position(self._elbow_empty_l.worldPosition, self.local_data['left_elbow_position']) self._set_object_position(self._elbow_empty_r.worldPosition, self.local_data['right_elbow_position']) self._set_object_position(self._shoulder_empty_l.worldPosition, self.local_data['left_shoulder_position']) self._set_object_position(self._shoulder_empty_r.worldPosition, self.local_data['right_shoulder_position']) self._set_object_position(self._hip_empty_l.worldPosition, self.local_data['left_hip_position']) self._set_object_position(self._hip_empty_r.worldPosition, self.local_data['right_hip_position']) self._set_object_position(self._knee_empty_l.worldPosition, self.local_data['left_knee_position']) self._set_object_position(self._knee_empty_r.worldPosition, self.local_data['right_knee_position']) self._set_object_position(self._foot_empty_l.worldPosition, self.local_data['left_foot_position']) self._set_object_position(self._foot_empty_r.worldPosition, self.local_data['right_foot_position'])
def __init__(self, obj, parent=None): logger.info('%s initialization' % obj.name) # Call the constructor of the parent class super(self.__class__, self).__init__(obj, parent) # Direction of the global vectors self.world_x_vector = mathutils.Vector([1, 0, 0]) self.world_y_vector = mathutils.Vector([0, 1, 0]) self.world_z_vector = mathutils.Vector([0, 0, 1]) self._destination = self.bge_object.position self._projection = self.bge_object.position self._wp_object = None self._collisions = False # Variable to store current speed. Used for the stop/resume services self._previous_speed = 0 # Choose the type of function to move the object #self._type = 'Velocity' self._type = 'Position' self.local_data['x'] = self._destination[0] self.local_data['y'] = self._destination[1] self.local_data['z'] = self._destination[2] # Waypoint tolerance (in meters) self.local_data['tolerance'] = 0.5 self.local_data['speed'] = self._speed # Identify an object as the target of the motion try: wp_name = self.bge_object['Target'] if wp_name != '': scene = blenderapi.scene() self._wp_object = scene.objects[wp_name] logger.info("Using object '%s' to indicate motion target" % wp_name) except KeyError: self._wp_object = None # Identify the collision detectors for the sides if self._obstacle_avoidance: for child in self.bge_object.children: if "Radar.R" in child.name: self._radar_r = child if "Radar.L" in child.name: self._radar_l = child try: logger.info("Radar Right is '%s'" % self._radar_r.name) logger.info("Radar Left is '%s'" % self._radar_l.name) self._collisions = True except AttributeError: logger.warning("No radars found attached to the waypoint \ actuator.There will be no obstacle avoidance") # Convert a string listing the properties to avoid into a list # Objects with any of these properties will be ignored # when doing obstacle avoidance try: props = self.bge_object['Ignore'].split(',') self.bge_object['Ignore'] = props except KeyError: self.bge_object['Ignore'] = [] logger.info('Component initialized')
def default_action(self): """ Move the object towards the destination. """ parent = self.robot_parent speed = self.local_data['speed'] v = 0 rz = 0 self._destination = [ self.local_data['x'], self.local_data['y'], self.local_data['z'] ] self._projection = [ self.local_data['x'], self.local_data['y'], self.bge_object.worldPosition[2] ] logger.debug("Robot {0} move status: '{1}'".format( parent.bge_object.name, parent.move_status)) # Place the target marker where the robot should go if self._wp_object: self._wp_object.position = self._destination # Vectors returned are already normalized projection_distance, projection_vector, local_vector = \ self.bge_object.getVectTo(self._projection) true_distance, global_vector, local_vector = \ self.bge_object.getVectTo(self._destination) # Convert to the Blender Vector object global_vector = mathutils.Vector(global_vector) projection_vector = mathutils.Vector(projection_vector) # if Z is not free, distance is the projection distance if self._free_z: distance = true_distance else: distance = projection_distance logger.debug("GOT DISTANCE: xy: %.4f ; xyz: %.4f" % (projection_distance, true_distance)) logger.debug("Global vector: %.4f, %.4f, %.4f" % (global_vector[0], global_vector[1], global_vector[2])) logger.debug("Local vector: %.4f, %.4f, %.4f" % (local_vector[0], local_vector[1], local_vector[2])) logger.debug( "Projection vector: %.4f, %.4f, %.4f" % (projection_vector[0], projection_vector[1], projection_vector[2])) # If the target has been reached, change the status if distance - self.local_data['tolerance'] <= 0: parent.move_status = "Arrived" #Do we have a running request? if yes, notify the completion self.completed(status.SUCCESS, parent.move_status) logger.debug("TARGET REACHED") logger.debug("Robot {0} move status: '{1}'".format( parent.bge_object.name, parent.move_status)) else: # Do nothing if the speed is zero if speed == 0: return parent.move_status = "Transit" angle_diff = 0 rotation_direction = 0 # If the projected distance is not null: else computing the # target angle is not possible! if projection_distance - self.local_data['tolerance'] / 2 > 0: ### Get the angle of the robot ### robot_angle = parent.position_3d.yaw ### Get the angle to the target ### target_angle = projection_vector.angle(self.world_x_vector) # Correct the direction of the turn according to the angles dot = projection_vector.dot(self.world_y_vector) logger.debug("Vector dot product = %.2f" % dot) if dot < 0: target_angle = target_angle * -1 ### Get the angle that the robot must turn ### if target_angle < robot_angle: angle_diff = robot_angle - target_angle rotation_direction = -1 else: angle_diff = target_angle - robot_angle rotation_direction = 1 # Make a correction when the angles change signs if angle_diff > math.pi: angle_diff = (2 * math.pi) - angle_diff rotation_direction = rotation_direction * -1 logger.debug( "Angles: R=%.4f, T=%.4f Diff=%.4f Direction = %d" % (robot_angle, target_angle, angle_diff, rotation_direction)) try: # Compute the speeds if self._type == 'Position': v = speed / self.frequency rotation_speed = (speed / self.frequency) / 2.0 elif self._type == 'Velocity': v = speed rotation_speed = 1.0 #speed / 2.0 # For the moment ignoring the division by zero # It happens apparently when the simulation starts except ZeroDivisionError: pass # Allow the robot to rotate in place if the waypoing is # to the side or behind the robot if angle_diff >= math.pi / 3.0: logger.debug("Turning on the spot!!!") v = 0 # Collision avoidance using the Blender radar sensor if self._collisions and v != 0 and self._radar_r['Rcollision']: # No obstacle avoidance when the waypoint is near if distance + self.local_data['tolerance'] > \ self._radar_r.sensors["Radar"].distance: # Ignore obstacles with the properties specified ignore = False for prop in self.bge_object['Ignore']: if prop in self._radar_r.sensors["Radar"].hitObject: ignore = True logger.debug( "Ignoring object '%s' " "with property '%s'" % (self._radar_r.sensors["Radar"].hitObject, prop)) break if not ignore: rz = rotation_speed logger.debug("Obstacle detected to the RIGHT, " "turning LEFT") elif self._collisions and v != 0 and self._radar_l['Lcollision']: # No obstacle avoidance when the waypoint is near if distance + self.local_data['tolerance'] > \ self._radar_l.sensors["Radar"].distance: # Ignore obstacles with the properties specified ignore = False for prop in self.bge_object['Ignore']: if prop in self._radar_l.sensors["Radar"].hitObject: ignore = True logger.debug("Ignoring object '%s' " "with property '%s'" % \ (self._radar_l.sensors["Radar"].hitObject, prop)) break if not ignore: rz = -rotation_speed logger.debug("Obstacle detected to the LEFT, " "turning RIGHT") # Test if the orientation of the robot is within tolerance elif -self._angle_tolerance < angle_diff < self._angle_tolerance: rz = 0 # If not, rotate the robot in the corresponding direction else: rz = rotation_speed * rotation_direction if self._free_z: vx = math.fabs(v * local_vector.dot(self.world_x_vector)) vz = v * local_vector.dot(self.world_z_vector) else: vx = v vz = 0 logger.debug( "Applying vx = %.4f, vz = %.4f, rz = %.4f (v = %.4f)" % (vx, vz, rz, v)) # Give the movement instructions directly to the parent # The second parameter specifies a "local" movement if self._type == 'Position': parent.bge_object.applyMovement([vx, 0, vz], True) parent.bge_object.applyRotation([0, 0, rz], True) elif self._type == 'Velocity': parent.bge_object.setLinearVelocity([vx, 0, vz], True) parent.bge_object.setAngularVelocity([0, 0, rz], True)
def translation(self): return mathutils.Vector((self.x, self.y, self.z))