public override IObservable <Space> Process(IObservable <Space> source) { return(Observable.Defer(() => { var handlers = collisionHandlers.ToDictionary(h => new CollisionHandlerKey(h.Material1, h.Material2)); var contacts = new ContactGeom[MaxContacts]; var collisionGroup = new JointGroup(); return source.Do(space => { collisionGroup.Clear(); space.Collide((g1, g2) => { var numContacts = Geom.Collide(g1, g2, contacts); if (numContacts == 0) { return; } var body1 = g1.Body; var body2 = g2.Body; var metadata1 = g1.Tag as GeomMetadata; var metadata2 = g2.Tag as GeomMetadata; if (metadata1 != null) { metadata1.OnCollision(g1, g2, contacts, numContacts); } if (metadata2 != null) { metadata2.OnCollision(g2, g1, contacts, numContacts); } CollisionHandler collisionHandler; if (metadata1 != null && metadata2 != null && handlers.TryGetValue(new CollisionHandlerKey(metadata1.Material, metadata2.Material), out collisionHandler)) { if (body1 != null || body2 != null) { if (ExcludeConnected && body1 != null && body2 != null && Body.AreConnectedExcluding(body1, body2, JointType.Contact)) { return; } var collisionSurface = collisionHandler.CollisionSurface; var world = body1 != null ? body1.World : body2.World; for (int i = 0; i < numContacts; i++) { var contactInfo = new ContactInfo(); contactInfo.Geometry = contacts[i]; contactInfo.Surface = collisionSurface; var contact = new Contact(world, contactInfo, collisionGroup); contact.Attach(body1, body2); } } } }); }).Finally(collisionGroup.Clear); })); }
public MoveItClient() { rosSocket = new RosSocket(new RosSharp.RosBridgeClient.Protocols.WebSocketNetProtocol(uri), RosSocket.SerializerEnum.Newtonsoft_JSON); /* * TRAJECTORY SETTING */ jointGroup = new JointGroup { name = "panda_arm_hand", jointNames = new string[] { "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", "panda_finger_joint2" } }; /* * ENVIRONMENT SETTING */ AddCollisionObjects("Box", boxPosition[0], boxPosition[1], boxPosition[2]); AddCollisionObjects("Screen", screenPosition[0], screenPosition[1], screenPosition[2]); AddCollisionObjects("Inducter", inducterPosition[0], inducterPosition[1], inducterPosition[2]); AddCollisionObjects("Arch", archXPosition, inducterPosition[1], inducterPosition[2]); AddCollisionObjects("Stick", archXPosition, inducterPosition[1], inducterPosition[2]); /* * INITIALIZE TRAJECTORY DICTIONARY */ trajectoryDict = new Dictionary <int, Dictionary <int, object> >(); /* * INITIALIZE CLIENT */ moveGroupActionClient = new MoveGroupActionClient(actionName, jointGroup, plannerID, rosSocket); moveGroupActionClient.PlanOnly = false; moveGroupActionClient.Initialize(); /* * MOVE TO INITIAL POSITION */ MoveToInitialPosition(); /* * PLANNER PARAMETRIZATION */ //plannerID = "ChompDefault"; //SetPlannerParameters(); //GetPlannerParameters(); }
internal Wrapper(JointGroup joints) { this.value = joints; }