//jointIndex = position in the rope
GameObject CreateRopeSection(int jointIndex)
{
GameObject joint = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
joint.transform.parent = this.transform;
joint.transform.localScale = new Vector3(jointDiameterCM / 100, jointLengthCM / 100, jointDiameterCM / 100);
joint.transform.localPosition = new Vector3(0, jointIndex * -((jointLengthCM / 100 + 0.01f) * 2), 0);
// *** Add ConfigurableJoint and configure it to the correct settings ***
ConfigurableJoint configJoint = joint.AddComponent <ConfigurableJoint>();
configJoint.enableCollision = true;
//Prevent object from spasming and flying away on collisions
configJoint.projectionMode = JointProjectionMode.PositionAndRotation;
joint.AddComponent <RopeJointStabilizer>();
//Connect the joint to the previous joint, if there is one
Debug.Log(joints.Count);
if (jointIndex == 0)
{
configJoint.connectedBody = hookHead.GetComponent <Rigidbody>();
}
if (jointIndex != 0)
{
//Connect the previous body to the joint
configJoint.connectedBody = joints[jointIndex - 1].GetComponent <Rigidbody>();
}
//Lock the motion to ensure the joints follow eachother
configJoint.xMotion = ConfigurableJointMotion.Locked;
configJoint.yMotion = ConfigurableJointMotion.Locked;
configJoint.zMotion = ConfigurableJointMotion.Locked;
// ** Configure rigidbody settings **
Rigidbody rb = joint.GetComponent <Rigidbody>();
rb.mass = CalcWeight();
rb.drag = 0.05f;
rb.interpolation = RigidbodyInterpolation.Interpolate;
rb.collisionDetectionMode = CollisionDetectionMode.Continuous;
// Add and configure Rotation script
rot rotScript = joint.AddComponent <rot>();
rotScript.center = rotCenter.transform;
return(joint);
}
void Update()
{
Hand = GetComponentInChildren <rot>();
rig.velocity = new Vector2(joystick.Horizontal * speed, joystick.Vertical * speed);
if ((joystick.Horizontal < 0 && (FacingRight == true && Hand.aimed_check == false)) || (Hand.dist < 0 && FacingRight == true && Hand.aim != Hand.Player))
{
flip();
}
if ((joystick.Horizontal > 0 && (FacingRight == false && Hand.aimed_check == false)) || (Hand.dist > 0 && FacingRight == false && Hand.aim != Hand.Player))
{
flip();
}
}
private void GetPointcloud(Image colorImage, Image depth, Image xyzImage, robotState state)
{
flag = false;
times++; //make sure this program has been processed once
//give out the tcp value and the tranformation matrix
if (state.cartesianInfo != null)
{
tcp = new Vector6 <double>();
ROT = new rot();
tcp = state.cartesianInfo.tcp;
ROT.Rx = state.cartesianInfo.tcp.Rx;
ROT.Ry = state.cartesianInfo.tcp.Ry;
ROT.Rz = state.cartesianInfo.tcp.Rz;
matrix_R = RobMath.RotVec2Matrix(ROT);
matrix_T_tcp2base.SetSubMatrix(0, 3, 0, 3, matrix_R);
matrix_T_tcp2base[3, 0] = 0;
matrix_T_tcp2base[3, 1] = 0;
matrix_T_tcp2base[3, 2] = 0;
matrix_T_tcp2base[3, 3] = 1;
matrix_T_tcp2base[0, 3] = tcp.X;
matrix_T_tcp2base[1, 3] = tcp.Y;
matrix_T_tcp2base[2, 3] = tcp.Z;
matrix_T = (DenseMatrix)(matrix_T_tcp2base * matrix_T_tool2tcp);
//info.Text = tcp.ToString();
//info.Text = matrix_T_tcp2base.ToString();
BGRA[] colorArray = colorImage.GetPixels <BGRA>().ToArray();
Short3[] xyzArray = xyzImage.GetPixels <Short3>().ToArray();
MathNet.Numerics.LinearAlgebra.Double.Matrix matrix_P1 = new DenseMatrix(4, 1);
MathNet.Numerics.LinearAlgebra.Double.Matrix matrix_P0 = new DenseMatrix(4, 1);
Vector3[] vertices = new Vector3[num];
Color32[] colors = new Color32[num];
//indices = new int[num];
//描画する点の配列番号を記録。(全ての点を描画)
//for (int i = 0; i < num; i++)
//{
// indices[i] = i;
//}
for (int i = 0; i < num; i++)
{
//頂点座標の代入
vertices[i].x = xyzArray[i].X * 0.001f;
vertices[i].y = xyzArray[i].Y * 0.001f;
vertices[i].z = xyzArray[i].Z * 0.001f;
//色の代入
colors[i].b = colorArray[i].B;
colors[i].g = colorArray[i].G;
colors[i].r = colorArray[i].R;
colors[i].a = 255;
////for each point-1 , use the T-matrix to tansform it to point-0
matrix_P1[0, 0] = vertices[i].x;
matrix_P1[1, 0] = vertices[i].y;
matrix_P1[2, 0] = vertices[i].z;
matrix_P1[3, 0] = 1;
matrix_P0 = (DenseMatrix)(matrix_T * matrix_P1);
//according to the position of the arm to save the pointcloud data
switch (position)
{
case 1:
{
vertices_0_b1[i].x = (float)matrix_P0[0, 0];
vertices_0_b1[i].y = (float)matrix_P0[1, 0];
vertices_0_b1[i].z = (float)matrix_P0[2, 0];
colors_0_b1[i] = colors[i];
}
break;
case 2:
{
vertices_0_b2[i].x = (float)matrix_P0[0, 0];
vertices_0_b2[i].y = (float)matrix_P0[1, 0];
vertices_0_b2[i].z = (float)matrix_P0[2, 0];
colors_0_b2[i] = colors[i];
}
break;
case 3:
{
vertices_0_b3[i].x = (float)matrix_P0[0, 0];
vertices_0_b3[i].y = (float)matrix_P0[1, 0];
vertices_0_b3[i].z = (float)matrix_P0[2, 0]; colors_0_b1[i] = colors[i];
colors_0_b3[i] = colors[i];
}
break;
case 4:
{
vertices_0_b4[i].x = (float)matrix_P0[0, 0];
vertices_0_b4[i].y = (float)matrix_P0[1, 0];
vertices_0_b4[i].z = (float)matrix_P0[2, 0];
colors_0_b4[i] = colors[i];
}
break;
case 5:
{
vertices_0_b5[i].x = (float)matrix_P0[0, 0];
vertices_0_b5[i].y = (float)matrix_P0[1, 0];
vertices_0_b5[i].z = (float)matrix_P0[2, 0];
colors_0_b5[i] = colors[i];
}
break;
}
}
info.Text = "times:" + times.ToString() + "," + "position:" + position.ToString() + "," + "scanning and processing done!";
}
else
{
info.Text = "times:" + times.ToString() + "state.cartesianInfo is null!";
}
}
void Start()
{
Hand = GetComponent <rot>();
}
