void Start()
{
if (this.rosbridgeIP.Equals(string.Empty))
{
this.rosbridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.sigverseBridgePort == 0)
{
this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClient = new System.Net.Sockets.TcpClient(this.rosbridgeIP, this.sigverseBridgePort);
Debug.Log("Connected=" + this.tcpClient.Connected);
this.networkStream = this.tcpClient.GetStream();
this.networkStream.ReadTimeout = 100000;
this.networkStream.WriteTimeout = 100000;
// RGB Camera
this.xtionRGBCamera = SIGVerseUtils.FindTransformFromChild(this.transform.root, "camera_rgb_optical_frame").GetComponentInChildren <Camera>();
int imageWidth = this.xtionRGBCamera.targetTexture.width;
int imageHeight = this.xtionRGBCamera.targetTexture.height;
this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false);
// [camera/rgb/CameraInfo]
string distortionModel = "plumb_bob";
double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
double[] K = { 570.3422241210938, 0.0, 319.5, 0.0, 570.3422241210938, 239.5, 0.0, 0.0, 1.0 };
double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
double[] P = { 570.3422241210938, 0.0, 319.5, 0.0, 0.0, 570.3422241210938, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false);
this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi);
// [camera/rgb/Image_raw]
string encoding = "rgb8";
byte isBigendian = 0;
uint step = (uint)imageWidth * 3;
this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null);
this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), "camera_rgb_optical_frame");
this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", this.topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData);
this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", this.topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData);
}
public static List <Transform> GetLinksInChildren(Transform root)
{
List <Transform> linkList = new List <Transform>();
foreach (string linkName in Enum.GetNames(typeof(Link)))
{
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, linkName));
}
return(linkList);
}
private bool SetSourceTransform()
{
if (!this.isServer)
{
return(false);
}
//Player[] players = PhotonNetwork.PlayerListOthers;
//int numberOfAvatars = players.Count(p => p.NickName.StartsWith("Human"));
if (PhotonNetwork.PlayerListOthers.Count(p => p.NickName.StartsWith(this.imitatorTargetPrefix)) == 0)
{
Debug.LogWarning("Imitator: source not found.");
return(false);
}
//this.srcAvatar = GameObject.FindGameObjectWithTag("Avatar").transform; // TODO: target should be selected in some way (e.g., a config value)
GameObject[] avatars = GameObject.FindGameObjectsWithTag("Avatar");
foreach (GameObject avatar in avatars)
{
if (avatar.name.StartsWith(this.imitatorTargetPrefix))
{
this.srcAvatar = avatar.transform;
break;
}
}
if (this.srcAvatar == null)
{
Debug.LogWarning("Imitator: source not found.");
return(false);
}
if (this.srcAvatar.GetComponent <HumanAvatarInitializer>().IsInitialized())
{
List <Transform> transforms = new List <Transform>();
foreach (Transform dsrTransform in destTransforms)
{
transforms.Add(SIGVerseUtils.FindTransformFromChild(this.srcAvatar.transform, dsrTransform.name));
}
srcTransforms = transforms.ToArray();
List <Transform> transformsRot = new List <Transform>();
foreach (Transform dsrTransformRot in destTransformsRot)
{
transformsRot.Add(SIGVerseUtils.FindTransformFromChild(this.srcAvatar.transform, dsrTransformRot.name));
}
srcTransformsRot = transformsRot.ToArray();
return(true);
}
return(false);
}
public void SetPlaybackTargets()
{
this.targetTransforms = new List <Transform>();
// Robot
this.targetTransforms.Add(this.robot.transform);
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.base_footprint.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.arm_lift_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.arm_flex_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.arm_roll_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.wrist_flex_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.wrist_roll_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.head_pan_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.head_tilt_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.torso_lift_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_motor_dummy_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_l_proximal_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_r_proximal_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_l_distal_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_r_distal_link.ToString()));
// Avatar
Transform avatar = GameObject.FindGameObjectWithTag("Avatar").transform;
Transform[] avatarTransforms = avatar.GetComponentsInChildren <Transform>(true);
foreach (Transform avatarTransform in avatarTransforms)
{
bool isTagergetTransform = true;
foreach (string keyword in KeywordsOfAvatarPartsPathToIgnore)
{
if (SIGVerse.Common.SIGVerseUtils.GetHierarchyPath(avatarTransform).Contains(keyword))
{
isTagergetTransform = false;
}
}
if (isTagergetTransform)
{
this.targetTransforms.Add(avatarTransform);
}
}
// Additional
foreach (string playbackTargetTag in this.playbackTargetTags)
{
GameObject[] playbackTargetObjects = GameObject.FindGameObjectsWithTag(playbackTargetTag);
foreach (GameObject playbackTargetObject in playbackTargetObjects)
{
this.targetTransforms.Add(playbackTargetObject.transform);
}
}
}
public static string GetDefinitionLine(List <VideoPlayer> targetVideoPlayers)
{
string definitionLine = "0.0," + WorldPlaybackCommon.DataType1VideoPlayer + "," + WorldPlaybackCommon.DataType2VideoPlayerDef; // Elapsed time is dummy.
foreach (VideoPlayer targetVideoPlayer in targetVideoPlayers)
{
// Make a header line
definitionLine += "\t" + SIGVerseUtils.GetHierarchyPath(targetVideoPlayer.transform);
}
return(definitionLine);
}
void Awake()
{
this.armLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.ArmLiftLinkName);
this.armFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.ArmFlexLinkName);
this.armRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.ArmRollLinkName);
this.wristFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.WristFlexLinkName);
this.wristRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.WristRollLinkName);
this.headPanLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.HeadPanLinkName);
this.headTiltLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.HeadTiltLinkName);
this.armLiftLinkIniPosZ = this.armLiftLink.localPosition.z;
}
public static string GetDefinitionLine(List <Transform> targetTransforms)
{
string definitionLine = "0.0," + WorldPlaybackCommon.DataType1Transform + "," + WorldPlaybackCommon.DataType2TransformDef; // Elapsed time is dummy.
foreach (Transform targetTransform in targetTransforms)
{
// Make a header line
definitionLine += "\t" + SIGVerseUtils.GetHierarchyPath(targetTransform);
}
return(definitionLine);
}
public static List <Transform> GetLinksInChildren(Transform root)
{
List <Transform> linkList = new List <Transform>();
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseFootPrintName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, ArmLiftLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, ArmFlexLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, ArmRollLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, WristFlexLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, WristRollLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandPalmLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandCameraFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandCameraRgbFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLProximalLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLSpringProximalLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLMimicDistalLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLDistalLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLFingerTipFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLFingerVacuumFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandMotorDummyLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRProximalLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRSpringProximalLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRMimicDistalLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRDistalLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRFingerTipFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, WristFtSensorFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseImuFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRangeSensorLink));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRollLlinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLDriveWheelLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLPassiveWheelXFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLPassiveWheelYFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLPassiveWheelZLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRDriveWheelLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRPassiveWheelXFramName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRPassiveWheelYFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRPassiveWheelZLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, TorsoLiftLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadPanLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadTiltLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadCenterCameraFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadCenterCameraRgbFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadLStereoCameraLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadLStereoCameraRgbFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRStereoCameraLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRStereoCameraRgbFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRgbdSensorLinkName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRgbdSensorRgbFrameName));
AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRgbdSensorDepthFrameName));
return(linkList);
}
protected override void Awake()
{
if (CleanupConfig.Instance.configFileInfo.playbackType == WorldPlaybackCommon.PlaybackTypePlay)
{
// Activate all grasping candidates
GameObject graspingCandidatesObj = GameObject.Find("GraspingCandidates");
foreach (Transform graspingCandidate in graspingCandidatesObj.transform)
{
graspingCandidate.gameObject.SetActive(true);
graspingCandidate.position = new Vector3(0.0f, -5.0f, 0.0f); // Wait in the ground
// Disable rigidbodies
Rigidbody[] rigidbodies = graspingCandidate.GetComponentsInChildren <Rigidbody>(true);
foreach (Rigidbody rigidbody in rigidbodies)
{
rigidbody.isKinematic = true;
}
// Disable colliders
Collider[] colliders = graspingCandidate.GetComponentsInChildren <Collider>(true);
foreach (Collider collider in colliders)
{
collider.enabled = false;
}
}
}
base.Awake();
// Robot
Transform robot = GameObject.FindGameObjectWithTag("Robot").transform;
this.targetTransforms.Add(robot);
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.base_footprint.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.arm_lift_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.arm_flex_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.arm_roll_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.wrist_flex_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.wrist_roll_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.head_pan_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.head_tilt_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.torso_lift_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_motor_dummy_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_l_proximal_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_r_proximal_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_l_distal_link.ToString()));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_r_distal_link.ToString()));
}
public PlaybackVideoPlayerEventController(WorldPlaybackCommon playbackCommon)
{
this.common = playbackCommon;
// Video Player
this.targetVideoPlayers = this.common.GetTargetVideoPlayers();
this.targetVideoPlayerPathMap = new Dictionary <string, VideoPlayer>();
foreach (VideoPlayer targetVideoPlayer in this.targetVideoPlayers)
{
this.targetVideoPlayerPathMap.Add(SIGVerseUtils.GetHierarchyPath(targetVideoPlayer.transform), targetVideoPlayer);
}
}
void Awake()
{
this.trajectoryInfoMap = new Dictionary <string, TrajectoryInfo>();
this.trajectoryInfoMap.Add(HSRCommon.OmniOdomX_JointName, null);
this.trajectoryInfoMap.Add(HSRCommon.OmniOdomY_JointName, null);
this.trajectoryInfoMap.Add(HSRCommon.OmniOdomT_JointName, null);
this.baseFootprintPosNoise = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintPosNoiseName);
this.baseFootprintRotNoise = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRotNoiseName);
this.baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRigidbodyName);
this.initialPosition = this.baseFootprintRigidbody.position;//save initial position.
this.initialRotation = this.baseFootprintRigidbody.rotation;
}//Awake
void Awake()
{
this.armLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_lift_link.ToString());
this.armFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_flex_link.ToString());
this.armRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_roll_link.ToString());
this.wristFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.wrist_flex_link.ToString());
this.wristRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.wrist_roll_link.ToString());
this.headPanLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_pan_link.ToString());
this.headTiltLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_tilt_link.ToString());
this.handLProximalLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.hand_l_proximal_link.ToString());
this.handRProximalLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.hand_r_proximal_link.ToString());
this.armLiftLinkIniPosZ = this.armLiftLink.localPosition.z;
}
protected void Awake()
{
this.collisionEffect = (GameObject)Resources.Load(CompetitionUtils.CollisionEffectPath);
GameObject robot = GameObject.FindGameObjectWithTag(TagRobot);
this.hsrBaseColliders = new List <Collider>();
this.hsrBaseColliders.AddRange(SIGVerseUtils.FindTransformFromChild(robot.transform.root, HSRCommon.BaseName).GetComponentsInChildren <Collider>());
this.hsrBaseColliders.AddRange(SIGVerseUtils.FindTransformFromChild(robot.transform.root, HSRCommon.BumperName).GetComponentsInChildren <Collider>());
this.hsrHandColliders = new List <Collider>();
this.hsrHandColliders.AddRange(SIGVerseUtils.FindTransformFromChild(robot.transform.root, HSRCommon.WristRollLinkName).GetComponentsInChildren <Collider>());
this.hsrGraspingDetector = robot.GetComponent <HSRGraspingDetector>();
}
protected static string GetDataLine(string elapsedTime, Collision collision, float collisionVelocity, float effectScale, string dataType)
{
Vector3 contactAve = SIGVerseUtils.CalcContactAveragePoint(collision);
string dataLine = elapsedTime + "," + dataType;
dataLine += "\t" + contactAve.x + "\t" + contactAve.y + "\t" + contactAve.z + "\t" + effectScale;
// Following data is unused now
dataLine += "\t" + collisionVelocity +
"\t" + SIGVerseUtils.GetHierarchyPath(collision.contacts[0].thisCollider.transform) +
"\t" + SIGVerseUtils.GetHierarchyPath(collision.contacts[0].otherCollider.transform);
return(dataLine);
}
public IEnumerator DisplayEndMessage()
{
yield return(new WaitForSecondsRealtime(7));
string endMessage = "All sessions have ended";
SIGVerseLogger.Info(endMessage);
this.tool.AddSpeechQueModerator(endMessage);
string panelMessage = endMessage + "\n"
+ "(" + SIGVerseUtils.GetOrdinal(HandymanConfig.Instance.numberOfTrials) + ": " + this.lastPanelMessage.Replace("\n", " - ") + ")";
this.SendPanelNotice(panelMessage, 80, PanelNoticeStatus.Blue, false);
this.bgmAudioSource.enabled = false;
}
void Awake()
{
this.baseFootprint = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.Link.base_footprint.ToString());
this.baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRigidbodyName);
this.baseFootprintPosNoise = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintPosNoiseName);
this.baseFootprintRotNoise = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRotNoiseName);
this.twist = this.transform.GetComponentInChildren <HSRSubTwist>(true);
this.trajectoryInfoMap = new Dictionary <HSRCommon.Joint, TrajectoryData>()
{
{ HSRCommon.Joint.odom_x, null },
{ HSRCommon.Joint.odom_y, null },
{ HSRCommon.Joint.odom_t, null },
};
}
//対象物体のパスとトランスフォームのマップを作成
private void SetTargetObjectPathTranformMap()
{
this.targetPathTransformMap.Clear();
//アバタの各部位
foreach (Transform transform in this.instructor.GetComponentsInChildren <Transform>())
{
this.targetPathTransformMap.Add(SIGVerseUtils.GetHierarchyPath(transform), transform);
}
//対象物体
foreach (GameObject targetObj in this.playbackCommon.GetTargetObjects())
{
this.targetPathTransformMap.Add(SIGVerseUtils.GetHierarchyPath(targetObj.transform), targetObj.transform);
}
}
void Awake()
{
this.torsoLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.torso_lift_link.ToString());
this.headPanLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_pan_link.ToString());
this.headTiltLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_tilt_link.ToString());
this.lShoulderPanLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_shoulder_pan_link.ToString());
this.lShoulderLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_shoulder_lift_link.ToString());
this.lUpperArmRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_upper_arm_roll_link.ToString());
this.lElbowFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_elbow_flex_link.ToString());
this.lForearmRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_forearm_roll_link.ToString());
this.lWristFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_wrist_flex_link.ToString());
this.lWristRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_wrist_roll_link.ToString());
this.rShoulderPanLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_shoulder_pan_link.ToString());
this.rShoulderLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_shoulder_lift_link.ToString());
this.rUpperArmRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_upper_arm_roll_link.ToString());
this.rElbowFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_elbow_flex_link.ToString());
this.rForearmRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_forearm_roll_link.ToString());
this.rWristFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_wrist_flex_link.ToString());
this.rWristRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_wrist_roll_link.ToString());
this.torsoLiftLinkIniPosZ = this.torsoLiftLink.localPosition.z;
this.trajectoryInfoMap = new Dictionary <PR2Common.Joint, TrajectoryInfo>()
{
{ PR2Common.Joint.torso_lift_joint, null },
{ PR2Common.Joint.head_pan_joint, null },
{ PR2Common.Joint.head_tilt_joint, null },
{ PR2Common.Joint.l_shoulder_pan_joint, null },
{ PR2Common.Joint.l_shoulder_lift_joint, null },
{ PR2Common.Joint.l_upper_arm_roll_joint, null },
{ PR2Common.Joint.l_elbow_flex_joint, null },
{ PR2Common.Joint.l_forearm_roll_joint, null },
{ PR2Common.Joint.l_wrist_flex_joint, null },
{ PR2Common.Joint.l_wrist_roll_joint, null },
{ PR2Common.Joint.r_shoulder_pan_joint, null },
{ PR2Common.Joint.r_shoulder_lift_joint, null },
{ PR2Common.Joint.r_upper_arm_roll_joint, null },
{ PR2Common.Joint.r_elbow_flex_joint, null },
{ PR2Common.Joint.r_forearm_roll_joint, null },
{ PR2Common.Joint.r_wrist_flex_joint, null },
{ PR2Common.Joint.r_wrist_roll_joint, null }
};
this.trajectoryKeyList = new List <PR2Common.Joint>(trajectoryInfoMap.Keys);
}
public PlaybackTransformEventController(WorldPlaybackCommon playbackCommon)
{
this.common = playbackCommon;
// Transform
this.targetTransforms = this.common.GetTargetTransforms();
this.targetTransformPathMap = new Dictionary <string, Transform>();
foreach (Transform targetTransform in this.targetTransforms)
{
this.targetTransformPathMap.Add(SIGVerseUtils.GetHierarchyPath(targetTransform), targetTransform);
}
this.IsRigidbodiesDisable = true;
this.IsCollidersDisable = true;
}
void Awake()
{
if (this.handType == HandType.Left)
{
this.gripperLFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_gripper_l_finger_link.ToString());
this.gripperRFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_gripper_r_finger_link.ToString());
this.gripperLFingerTipLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_gripper_l_finger_tip_link.ToString());
this.gripperRFingerTipLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_gripper_r_finger_tip_link.ToString());
}
else
{
this.gripperLFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_gripper_l_finger_link.ToString());
this.gripperRFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_gripper_r_finger_link.ToString());
this.gripperLFingerTipLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_gripper_l_finger_tip_link.ToString());
this.gripperRFingerTipLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_gripper_r_finger_tip_link.ToString());
}
}
private void PreProcess()
{
this.sessionManager.ChangeEnvironment(this.numberOfSession);
this.ResetAvatarTransform();
this.sessionManager.ActivateRobot();
if (!this.isPracticeMode)
{
this.rosConnections = SIGVerseUtils.FindObjectsOfInterface <IRosConnection>();
SIGVerseLogger.Info("ROS connection : count=" + this.rosConnections.Length);
}
else
{
SIGVerseLogger.Info("No ROS connection (Practice mode)");
}
//this.currentTaskInfo = this.sessionManager.GetCurrentTaskInfo();
this.currentTaskInfo = this.sessionManager.GetTaskInfo(this.numberOfSession);
this.taskInfoForRobot = new SIGVerse.RosBridge.human_navigation.HumanNaviTaskInfo();
string environmentName = this.sessionManager.GetEnvironment().name;
this.taskInfoForRobot.environment_id = environmentName.Substring(0, environmentName.Length - 3);
this.SetObjectListToHumanNaviTaskInfo();
this.SetFurnitureToHumanNaviTaskInfo();
this.SetDestinationToHumanNaviTaskInfo();
this.waitingTime = 0.0f;
this.interruptedReason = string.Empty;
this.objectIdInLeftHand = "";
this.objectIdInRightHand = "";
this.objectIdInLeftHandPreviousFrame = ""; // Tentative Code
this.objectIdInRightHandPreviousFrame = ""; // Tentative Code
this.alreadyGraspedObjects = new List <string>();
this.InitializePlayback();
//this.countWrongObjectsGrasp = 0;
// SIGVerseLogger.Info("End of PreProcess");
}
//録画を行う対象物体を格納
private void SetTargetTransforms()
{
this.targetTransformInstances.Clear();
//アバタの各部位
foreach (Transform avatarTransform in this.playbackCommon.GetAvatar().GetComponentsInChildren <Transform>())
{
if (!this.ignoringKeywords.Exists(x => SIGVerseUtils.GetHierarchyPath(avatarTransform).Contains(x)))
{
this.targetTransformInstances.Add(avatarTransform);
}
}
//対象物体
foreach (GameObject graspingCandidate in this.playbackCommon.GetTargetObjects())
{
this.targetTransformInstances.Add(graspingCandidate.transform);
}
}
void Awake()
{
this.torsoLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.torso_lift_link.ToString());
this.head1Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_1_link.ToString());
this.head2Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_2_link.ToString());
this.arm1Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_1_link.ToString());
this.arm2Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_2_link.ToString());
this.arm3Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_3_link.ToString());
this.arm4Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_4_link.ToString());
this.arm5Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_5_link.ToString());
this.arm6Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_6_link.ToString());
this.arm7Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_7_link.ToString());
this.gripperLeftFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.gripper_left_finger_link.ToString());
this.gripperRightFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.gripper_right_finger_link.ToString());
this.torsoLiftLinkIniPosZ = this.torsoLiftLink.localPosition.z;
this.arm1LinkIniRotZ = this.arm1Link.localEulerAngles.z;
this.arm3LinkIniRotZ = this.arm3Link.localEulerAngles.z;
this.arm4LinkIniRotY = this.arm4Link.localEulerAngles.y;
this.arm6LinkIniRotY = this.arm6Link.localEulerAngles.y;
this.arm7LinkIniRotZ = this.arm7Link.localEulerAngles.z;
this.gripperLeftFingerLinkIniPosX = this.gripperLeftFingerLink.localPosition.x;
this.gripperRightFingerLinkIniPosX = this.gripperRightFingerLink.localPosition.x;
this.trajectoryInfoMap = new Dictionary <TIAGoCommon.Joint, TrajectoryInfo>()
{
{ TIAGoCommon.Joint.torso_lift_joint, null },
{ TIAGoCommon.Joint.head_1_joint, null },
{ TIAGoCommon.Joint.head_2_joint, null },
{ TIAGoCommon.Joint.arm_1_joint, null },
{ TIAGoCommon.Joint.arm_2_joint, null },
{ TIAGoCommon.Joint.arm_3_joint, null },
{ TIAGoCommon.Joint.arm_4_joint, null },
{ TIAGoCommon.Joint.arm_5_joint, null },
{ TIAGoCommon.Joint.arm_6_joint, null },
{ TIAGoCommon.Joint.arm_7_joint, null },
{ TIAGoCommon.Joint.gripper_left_finger_joint, null },
{ TIAGoCommon.Joint.gripper_right_finger_joint, null },
};
this.trajectoryKeyList = new List <TIAGoCommon.Joint>(trajectoryInfoMap.Keys);
}
public void scoreCollision(GameObject obj, Collision collision)
{
if (recordScore && collision.relativeVelocity.magnitude > scoreThreshold)
{
//Arm-Obstacle
String colPathArm = SIGVerseUtils.GetHierarchyPath(collision.collider.transform);
bool colArm = colPathArm.Contains("arm_lift_link");
if (colArm)
{
//SIGVerseLogger.Info ("************************************************************************************************ ARM COLLISION ************" );
collisionA++;
}
//Obstacle-Obstacle
bool colObs = collision.gameObject.CompareTag(TagObstacleCandidates);
if (colObs)
{
//SIGVerseLogger.Info ("************************************************************************************************ OBSTACLE COLLISION ************" );
collisionO++;
}
//Target-Obstacle
bool colTar = collision.gameObject.CompareTag(TagGraspingCandidates);
if (colTar)
{
//SIGVerseLogger.Info ("************************************************************************************************ TARGET COLLISION ************" );
collisionT++;
}
//Furniture-Obstacle
String colPathFur = SIGVerseUtils.GetHierarchyPath(collision.collider.transform);
bool colFur = colPathFur.Contains("Tables");
if (colFur)
{
//SIGVerseLogger.Info ("************************************************************************************************ FURNITURE COLLISION ************" );
collisionF++;
}
if (collision.relativeVelocity.magnitude > maxVel)
{
maxVel = collision.relativeVelocity.magnitude;
}
}
}
void Awake()
{
try
{
// Playback system
this.playbackRecorder = this.playbackManager.GetComponent <HumanNaviPlaybackRecorder>();
// Avatar
this.initialAvatarPosition = this.avatar.transform.position;
this.initialAvatarRotation = this.avatar.transform.eulerAngles;
// GUI
this.mainMenu = GameObject.FindGameObjectWithTag("MainMenu");
this.panelMainController = mainMenu.GetComponent <PanelMainController>();
this.noticePanelForAvatar.SetActive(false);
this.noticeTextForAvatar.text = "";
// MessageMap
this.receivedMessageMap = new Dictionary <string, bool>();
this.receivedMessageMap.Add(MsgIamReady, false);
this.receivedMessageMap.Add(MsgGetAvatarPose, false);
this.receivedMessageMap.Add(MsgConfirmSpeechState, false);
this.receivedMessageMap.Add(MsgGiveUp, false);
// ROSBridge
// (Should be read after the robot is instantiated (after Awake process of SessionManager))
this.rosConnections = SIGVerseUtils.FindObjectsOfInterface <IRosConnection>();
SIGVerseLogger.Info("ROS connection : count=" + this.rosConnections.Length);
// Timer
this.stepTimer = new StepTimer();
}
catch (Exception exception)
{
Debug.LogError(exception);
SIGVerseLogger.Error(exception.Message);
SIGVerseLogger.Error(exception.StackTrace);
this.ApplicationQuitAfter1sec();
}
}
protected override void Awake()
{
base.Awake();
// Robot
Transform robot = GameObject.FindGameObjectWithTag("Robot").transform;
this.targetTransforms.Add(robot);
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.BaseFootPrintName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.ArmLiftLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.ArmFlexLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.ArmRollLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.WristFlexLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.WristRollLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.HeadPanLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.HeadTiltLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.TorsoLiftLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.HandLProximalLinkName));
this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.HandRProximalLinkName));
}
private void Awake()
{
try
{
// For practice mode
if (HumanNaviConfig.Instance.configInfo.executionMode == (int)ExecutionMode.Practice)
{
this.GetComponent <HumanNaviPubTaskInfo>().enabled = false;
this.GetComponent <HumanNaviPubMessage>().enabled = false;
this.GetComponent <HumanNaviSubMessage>().enabled = false;
this.GetComponent <HumanNaviPubAvatarStatus>().enabled = false;
this.GetComponent <HumanNaviPubObjectStatus>().enabled = false;
robot.transform.Find("RosBridgeScripts").gameObject.SetActive(false);
robot.GetComponentInChildren <HumanNaviSubGuidanceMessage>().enabled = false;
foreach (Camera camera in robot.transform.GetComponentsInChildren <Camera>())
{
camera.gameObject.SetActive(false);
}
}
this.robot.SetActive(false);
this.robotBaseFootprint = SIGVerseUtils.FindTransformFromChild(this.robot.transform, "base_footprint");
this.InitializeEnvironments();
this.tts = this.robot.transform.Find("CompetitionScripts").GetComponent <SAPIVoiceSynthesisExternal>();
this.InitializeTaskInfo();
}
catch (Exception e)
{
SIGVerseLogger.Error(e.Message);
this.ApplicationQuitAfter1sec();
}
}
void Awake()
{
List <UnityEngine.Transform> localLinkList = HSRCommon.GetLinksInChildren(this.transform.root);
foreach (UnityEngine.Transform localLink in localLinkList)
{
if (localLink.name == HSRCommon.Link.base_footprint.ToString())
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = HSRCommon.OdomName;
localTransformStamped.child_frame_id = localLink.name;
UnityEngine.Transform baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRigidbodyName);
TfInfo localTfInfo = new TfInfo(baseFootprintRigidbody, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
else
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = localLink.parent.name;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
}
this.synchronizer = this.GetComponent <HSRPubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
void Awake()
{
this.torsoLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.torso_lift_link.ToString());
this.head1Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_1_link.ToString());
this.head2Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_2_link.ToString());
this.arm1Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_1_link.ToString());
this.arm2Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_2_link.ToString());
this.arm3Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_3_link.ToString());
this.arm4Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_4_link.ToString());
this.arm5Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_5_link.ToString());
this.arm6Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_6_link.ToString());
this.arm7Link = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_7_link.ToString());
this.gripperLeftFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.gripper_left_finger_link.ToString());
this.gripperRightFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.gripper_right_finger_link.ToString());
this.torsoLiftLinkIniPosZ = this.torsoLiftLink.localPosition.z;
this.arm1LinkIniRotZ = this.arm1Link.localEulerAngles.z;
this.arm3LinkIniRotZ = this.arm3Link.localEulerAngles.z;
this.arm4LinkIniRotY = this.arm4Link.localEulerAngles.y;
this.arm6LinkIniRotY = this.arm6Link.localEulerAngles.y;
this.arm7LinkIniRotZ = this.arm7Link.localEulerAngles.z;
this.gripperLeftFingerLinkIniPosX = this.gripperLeftFingerLink.localPosition.x;
this.gripperRightFingerLinkIniPosX = this.gripperRightFingerLink.localPosition.x;
}
public PlaybackTransformEventController(string filePath)
{
this.targetTransforms = new List <Transform>();
foreach (GameObject rootObj in SceneManager.GetActiveScene().GetRootGameObjects())
{
if (!rootObj.activeInHierarchy)
{
continue;
}
this.targetTransforms.AddRange(rootObj.GetComponentsInChildren <Transform>(true));
}
this.targetTransformPathMap = new Dictionary <string, Transform>();
foreach (Transform targetTransform in this.targetTransforms)
{
this.targetTransformPathMap.Add(SIGVerseUtils.GetHierarchyPath(targetTransform), targetTransform);
}
this.IsRigidbodiesDisable = true;
this.IsCollidersDisable = true;
}