Vector3 Parent(JointState js, int i) {
if ( i >= 3 && i <= 5 ) return js.Pos("right-shoulder");
else if ( i >= 7 && i <= 9 ) return js.Pos("left-shoulder");
else if ( i >= 11 && i <= 13 ) return js.Pos("right-hip");
else if ( i >= 15 && i <= 17 ) return js.Pos("left-hip");
return js.NeckPos;
}
public float QueryFrame (JointState js)
{
/*var error_sum = 0.0f;
for ( int i = 0; i < js.RelativeJoints.Length; i++ ) {
var reljoint = (js.RelativeJoints[i] - Parent(js, i));
reljoint.NormalizeFast();
var error = (reljoint - mAverage[i]).LengthFast;
if ( error < mVariance[i] ) error_sum += 0.0f;
else error_sum += mWeights[i] * (error - mVariance[i]);
//error_sum += (1.0f - Utility.Sigmoid(error.LengthSq * mWeights[i] - mVariance[i] - 2.0f)) * mWeights[i];
}
var total_error = 1.0f - error_sum / mWeights.Sum();
Console.WriteLine("Total error: {0}", total_error);
return total_error;*/
var error_sum = 0.0f;
for ( int i = 0; i < js.RelativeJoints.Length; i++ ) {
var reljoint = (js.RelativeJoints[i] - Parent(js, i));
reljoint.NormalizeFast();
var error = (reljoint - mAverage[i]).LengthFast / 2.0f;
error_sum += (float)Math.Pow(error, 1.0f/mWeights[i]);
}
var conf = 1.0f - error_sum / mVariance.Sum() / mWeights.Sum();
//Console.WriteLine("Confidence: {0}", conf);
return conf;
}
// Use this for initialization
void Start()
{
List<AnotatedAnimation> aList = new List<AnotatedAnimation>();
AnotatedAnimation anotatedAnimation = new AnotatedAnimation();
anotatedAnimation.angle = 0.1F;
anotatedAnimation.distance = 1.0F;
anotatedAnimation.LeftFoot = new JointState[2];
JointState js = new JointState();
js.angle = 0.1F;
js.orientation = new Vector3(1.0F,1.0F,1.0F);
anotatedAnimation.LeftFoot[0] = js;
anotatedAnimation.LeftFoot[1] = js;
aList.Add(anotatedAnimation);
aList.Add(anotatedAnimation);
// writing an object to file
ObjectSerializer os = new ObjectSerializer();
os.serializedObject = aList;
os.writeObjectToFile("test.xml");
//reading an object from file
ObjectSerializer os1 = new ObjectSerializer();
os1.serializedObject = new List<AnotatedAnimation>();
os1.readObjectFromFile("test.xml");
List<AnotatedAnimation> objectThatNeedsToBeReadFromFile = (List<AnotatedAnimation>) os1.serializedObject;
Debug.Log("after read " + objectThatNeedsToBeReadFromFile[1].LeftFoot[1].orientation);
}
public static JointState emptyJointState()
{
JointState state = new JointState();
state.effort = new double[] { 5.34, 1.345, 2.4543, 2.3432, 0.45367 };
state.position = new double[] { 5.34, 1.345, 2.4543, 2.3432, 0.45367 };
state.velocity = new double[] { 5.34, 1.345, 2.4543, 2.3432, 0.45367 };
state.name = new string[] { "Heinz", "Peter", "Alfons", "Dieter", "Karl" };
return(state);
}
public float QueryFrame(JointState js)
{
if (js.Pos("right-foot").Y >= -0.2f)
{
return(1.0f);
}
else
{
return(0.0f);
}
}
public float QueryFrame(JointState js)
{
if (js.Pos("right-palm").Z > 0.5f)
{
return(1.0f);
}
else
{
return(0.0f);
}
}
public float QueryFrame(JointState js)
{
if ((js.Pos("right-palm") - js.Pos("left-palm")).Length < 0.2f)
{
return(1.0f);
}
else
{
return(0.0f);
}
}
protected override void OnLoad(EventArgs e)
{
base.OnLoad(e);
GL.ClearColor(0.2f, 0.2f, 0.2f, 1.0f);
GL.PointSize(8.0f);
GL.Enable(EnableCap.DepthTest);
mGesture = new InputGesture(new LogFileLoader(mLogFilename));
mCurrState = JointState.CloneFrom(mGesture.States[0]);
}
private void updateJointState(JointState jointState)
{
if (jointState_.name == null)
{
return;
}
for (int i = 0; i < jointState.name.Count; i++)
{
jointsGrid[2, i].Value =
AngleConverter.RadianToDegrees(jointState.position[i]).ToString("F2");
}
}
static void joint_states_cb(JointState joint_states)
{
Console.WriteLine("Got joint_states message: {0}", String.Join(", ", joint_states.position.Select(x => x.ToString())));
var js2 = new JointState();
js2.header = new Header();
js2.name = new string[] { "joint_1", "joint_2" };
js2.position = new double[] { joint_states.position[0] * 2.0, joint_states.position[1] * 4.0 };
pub.publish(js2);
}
public static JointState[] ROSReadArray(BinaryReader reader, int count)
{
if (count < 0)
{
count = (int)reader.ReadUInt32();
}
var o = new JointState[count];
for (int i = 0; i < count; i++)
{
o[i] = ROSRead(reader);
}
return(o);
}
public RosBridgeClient(bool verbose, bool imageStreaming, bool jointStates, bool testLatency, bool pointCloud)
{
RosBridgeClient.verbose = verbose;
RosBridgeClient.imageStreaming = imageStreaming;
RosBridgeClient.jointStates = jointStates;
RosBridgeClient.testLatency = testLatency;
latestImage = new CompressedImage(); // Buffer for latest incoming image message
latestJointState = new JointState(); // Buffer for latest incoming jointState
rosMessageStrings = new Queue <string> (); // Incoming message queue
rosMessageConverter = new RosMessageConverter(); // Deserializing of incoming ROSmessages
rosCommandQueue = new Queue <RosMessage> (); // Outgoing message queue
streamWriter = new StreamWriter("latencyData.txt");
latencyData = new List <string> ();
}
// die andere Richtung wäre auch schön, damit man auch Nachrichten an ROS senden kann
public string startSerializing(RosMessage message)
{
//todo
JointState js = new JointState();
RosPublish publishMessage = (RosPublish)message;
MessageData msg = publishMessage.msg;
if (msg.GetType() == typeof(JointState))
{
js = (JointState)msg;
}
return(serializeJointState(js));
}
public ArmPose GetArmPose()
{
Vector3 offsetPos = gripperOffset.transform.localPosition;
JointState gripperOffsetJoint = new JointState();
gripperOffsetJoint.translation = new Point(offsetPos.x, offsetPos.y, offsetPos.z);
ArmPose armPose = new ArmPose(
GetJointState(worldJoint, 0),
GetJointState(baseJoint, 1),
GetJointState(shoulderJoint, 3),
GetJointState(elbowJoint, 3),
GetJointState(wristJoint, 3),
GetJointState(effJoint, 1),
gripperOffsetJoint,
GetJointState(rightGripper, 3),
GetJointState(leftGripper, 3));
return armPose;
}
Vector3 Parent(JointState js, int i)
{
if (i >= 3 && i <= 5)
{
return(js.Pos("right-shoulder"));
}
else if (i >= 7 && i <= 9)
{
return(js.Pos("left-shoulder"));
}
else if (i >= 11 && i <= 13)
{
return(js.Pos("right-hip"));
}
else if (i >= 15 && i <= 17)
{
return(js.Pos("left-hip"));
}
return(js.NeckPos);
}
void Start()
{
if (this.rosBridgeIP.Equals(string.Empty))
{
this.rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.rosBridgePort == 0)
{
this.rosBridgePort = ConfigManager.Instance.configInfo.rosbridgePort;
}
this.webSocketConnection = new SIGVerse.ROSBridge.ROSBridgeWebSocketConnection(rosBridgeIP, rosBridgePort);
this.jointStatePublisher = this.webSocketConnection.Advertise <JointState>(topicName);
// Connect to ROSbridge server
this.webSocketConnection.Connect();
this.jointState = new JointState();
this.jointState.header = new Header(0, new SIGVerse.ROSBridge.msg_helpers.Time(0, 0), "hsrb_joint_states");
this.jointState.name = new List <string>();
this.jointState.name.Add(HSRCommon.ArmLiftJointName); //1
this.jointState.name.Add(HSRCommon.ArmFlexJointName); //2
this.jointState.name.Add(HSRCommon.ArmRollJointName); //3
this.jointState.name.Add(HSRCommon.WristFlexJointName); //4
this.jointState.name.Add(HSRCommon.WristRollJointName); //5
this.jointState.name.Add(HSRCommon.HeadPanJointName); //6
this.jointState.name.Add(HSRCommon.HeadTiltJointName); //7
this.jointState.position = null;
this.jointState.velocity = new List <double> {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
};
this.jointState.effort = new List <double> {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
};
}
private RosPublish deserializeJointState(JsonObject jsonObject)
{
JsonArray jnarray = jsonObject["msg"].GetObject()["name"].GetArray();
JsonArray jparray = jsonObject["msg"].GetObject()["position"].GetArray();
JsonArray jvarray = jsonObject["msg"].GetObject()["velocity"].GetArray();
JsonArray jearray = jsonObject["msg"].GetObject()["effort"].GetArray();
// baue den Header
Header header = RosHeaderCoder_.deserializeSingleHeader((JsonValue)jsonObject["msg"].GetObject()["header"]);
//Restliche Werte des JointState rausfinden
JointState messageData = new JointState();
messageData.header = header;
// Nutze den ArrayService, um die JSONArrays in "normale" Arrays umzuwandeln
messageData.name = RosArrayService_.stringArrayAusJSonArray(jnarray);
messageData.position = RosArrayService_.doubleArrayAusJSonArray(jparray);
messageData.velocity = RosArrayService_.doubleArrayAusJSonArray(jvarray);
messageData.effort = RosArrayService_.doubleArrayAusJSonArray(jearray);
return(new RosPublish("\"/joint_states\"", messageData));
}
public JointAnglesWithSpeedResult()
{
this.goal_position = new JointState();
}
public RosJointStateCoder_()
{
latestJointState = new JointState();
}
void SetNextFrame(double lastTime, PartId part, Frame last)
{
var next = frames.First(x => x.Part == part && x.Time > lastTime);
states[part] = new JointState(last, next);
}
private void OnUpdateJointState(JointState jointState)
{
jointState_ = jointState;
}
public JointTrajectoryResult()
{
this.goal_position = new JointState();
}
public JointTrajectoryResult(JointState goal_position)
{
this.goal_position = goal_position;
}
public LocomotionMode type; // the type of the animations
#endregion Fields
#region Methods
public void Init(int samples)
{
RootCurve.initRootPos = Vector3.zero;
RootCurve.xPos = new AnimationCurve();
RootCurve.yPos = new AnimationCurve();
RootCurve.zPos = new AnimationCurve();
RootCurve.yRot = new AnimationCurve();
Root = new JointState[samples];
LeftFoot = new JointState[samples];
RightFoot = new JointState[samples];
RightHand = new Vector3[samples];
LeftHand = new Vector3[samples];
SupportingFoot = new JointState[samples];
SwingFoot = new JointState[samples];
for (int i=0; i<samples; i++)
{
Root[i] = new JointState();
LeftFoot[i] = new JointState();
RightFoot[i] = new JointState();
SupportingFoot[i] = new JointState();
SwingFoot[i] = new JointState();
}
movement_LeftFoot = new JointState();
movement_RightFoot = new JointState();
}
public void SetSupportingFoot(Joint sup)
{
supporting = sup;
int samples = Root.Length;
if (supporting == Joint.LeftFoot)
{
swing = Joint.RightFoot;
for (int i=0; i<samples; i++)
{
SupportingFoot[i] = LeftFoot[i];
SwingFoot[i] = RightFoot[i];
}
movement_SupportingFoot = movement_LeftFoot;
movement_SwingFoot = movement_RightFoot;
}
else
{
swing = Joint.LeftFoot;
for (int i=0; i<samples; i++)
{
SupportingFoot[i] = RightFoot[i];
SwingFoot[i] = LeftFoot[i];
}
movement_SupportingFoot = movement_RightFoot;
movement_SwingFoot = movement_LeftFoot;
}
}
public JointAnglesWithSpeedResult(JointState goal_position)
{
this.goal_position = goal_position;
}
protected void DigestJointStateMessage(JointState state)
{
State.JointSate = state;
State.JointStateHasChanged = true;
}
