robowflex::TF Namespace Reference

Collection of methods relating to transforms and transform math. More...

Functions
RobotPose	identity ()
	Creates the Identity pose. More...
RobotPose	createPoseXYZ (double x, double y, double z)
	Creates a robot pose from a linear component and zero orientation. More...
RobotPose	createPoseXYZ (const Eigen::Ref< const Eigen::Vector3d > &translation)
	Creates a robot pose from a linear component and zero orientation. More...
RobotPose	createPoseXYZ (double x, double y, double z, double X, double Y, double Z)
	Creates a robot pose from a linear component and XYZ convention Euler angles. More...
RobotPose	createPoseXYZ (const Eigen::Ref< const Eigen::Vector3d > &translation, const Eigen::Ref< const Eigen::Vector3d > &rotation)
	Creates a robot pose from a linear component and XYZ convention Euler angles. More...
RobotPose	createPoseQ (double x, double y, double z, double W, double X, double Y, double Z)
	Creates a robot pose from a linear component and a Quaternion. More...
RobotPose	createPoseQ (const Eigen::Ref< const Eigen::Vector3d > &translation, const Eigen::Ref< const Eigen::Vector4d > &rotation)
	Creates a robot pose from a linear component and a quaternion. More...
RobotPose	createPoseQ (const Eigen::Ref< const Eigen::Vector3d > &translation, const Eigen::Quaterniond &rotation)
	Creates a robot pose from a linear component and a quaternion. More...
Eigen::Quaterniond	getPoseRotation (const RobotPose &pose)
	Get the rotational component of a robot pose. More...
Eigen::Vector3d	pointMsgToEigen (const geometry_msgs::Point &msg)
	Converts a point message to an Eigen::Vector3d. More...
geometry_msgs::Point	pointEigenToMsg (const Eigen::Vector3d &vector)
	Converts an Eigen::Vector3d to a point message. More...
Eigen::Vector3d	vectorMsgToEigen (const geometry_msgs::Vector3 &msg)
	Converts a vector message to an Eigen::Vector3d. More...
geometry_msgs::Vector3	vectorEigenToMsg (const Eigen::Vector3d &vector)
	Converts an Eigen::Vector3d to a vector message. More...
RobotPose	poseMsgToEigen (const geometry_msgs::Pose &msg)
	Converts a pose message to RobotPose. More...
geometry_msgs::Pose	poseEigenToMsg (const RobotPose &pose)
	Converts an RobotPose to a pose message. More...
Eigen::Quaterniond	quaternionMsgToEigen (const geometry_msgs::Quaternion &msg)
	Converts a quaternion message to Eigen::Quaterniond. More...
geometry_msgs::Quaternion	quaternionEigenToMsg (const Eigen::Quaterniond &quaternion)
	Converts an Eigen::Quaterniond to a quaternion message. More...
moveit_msgs::BoundingVolume	getBoundingVolume (const RobotPose &pose, const GeometryConstPtr &geometry)
	Get a bounding volume message for given geometry at a pose. More...
moveit_msgs::PositionConstraint	getPositionConstraint (const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &geometry)
	Get a position constraint message. More...
Eigen::Vector3d	samplePositionConstraint (const moveit_msgs::PositionConstraint &pc)
moveit_msgs::OrientationConstraint	getOrientationConstraint (const std::string &ee_name, const std::string &base_name, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances)
	Get an orientation constraint message. More...
Eigen::Quaterniond	sampleOrientation (const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances)
	Sample an orientation from a given orientation with XYZ Euler angle tolerances. More...
Eigen::Quaterniond	sampleOrientationUniform (const Eigen::Vector3d &tolerances)
	Sample an orientation within the XYZ Euler angle tolerances. More...
Eigen::Quaterniond	offsetOrientation (const Eigen::Quaterniond &orientation, const Eigen::Vector3d &axis, double value)
	Offset an orientation by a rotation about an axis. More...
Eigen::Vector3d	samplePositionUniform (const Eigen::Vector3d &bounds)
	Sample a position within the given bounds using a uniform distribution. More...
Eigen::Vector3d	samplePositionGaussian (const Eigen::Vector3d &stddev)
	Sample a position from a gaussian distribution with mean zero and given standard deviation. More...
RobotPose	samplePoseUniform (const Eigen::Vector3d &pos_bounds, const Eigen::Vector3d &orn_bounds)
	Sample a pose within the given position, orientation bounds. More...
RobotPose	samplePoseGaussian (const Eigen::Vector3d &pos_stddev, const Eigen::Vector3d &orn_bounds)
	Sample a pose with gaussian sampling for the position with given standard deviations and uniform sampling for the orientation within the given bounds. More...
RobotPose	transformMsgToEigen (const geometry_msgs::TransformStamped &tf)
	Decode a message as a transform. More...
geometry_msgs::TransformStamped	transformEigenToMsg (const std::string &source, const std::string &target, const RobotPose &tf)
	Encode a transform as a message. More...
double	angleNormalize (double v)
	Normalize an angle between -pi to pi. More...
double	toDegrees (double v)
	Convert an angle to degrees. More...
double	toRadians (double v)
	Convert an angle to radians. More...
bool	isVecZero (const Eigen::Ref< const Eigen::VectorXd > &v, double tolerance=constants::eps)
	Checks if a vector is close to zero. More...

Detailed Description

Collection of methods relating to transforms and transform math.

Function Documentation

angleNormalize()

double robowflex::TF::angleNormalize

(

double

v

)

Normalize an angle between -pi to pi.

Parameters

[in]

v

The angle.

Returns

The normalized angle.

Definition at line 293 of file tf.cpp.

{
    return (v > constants::pi) ? constants::two_pi - v : v;
}

createPoseQ() [1/3]

RobotPose robowflex::TF::createPoseQ	(	const Eigen::Ref< const Eigen::Vector3d > &	translation,
		const Eigen::Quaterniond &	rotation
	)

Creates a robot pose from a linear component and a quaternion.

Parameters

[in]	translation	translation component.
[in]	rotation	rotational component.

Returns

A new robot pose from components.

Definition at line 59 of file tf.cpp.

{
    RobotPose pose = RobotPose::Identity();
    pose.translation() = translation;
    pose.linear() = rotation.toRotationMatrix();
 
    return pose;
}

createPoseQ() [2/3]

RobotPose robowflex::TF::createPoseQ	(	const Eigen::Ref< const Eigen::Vector3d > &	translation,
		const Eigen::Ref< const Eigen::Vector4d > &	rotation
	)

Creates a robot pose from a linear component and a quaternion.

Parameters

[in]	translation	translation component.
[in]	rotation	rotational component (W, X, Y, Z quaternion values).

Returns

A new robot pose from components.

Definition at line 53 of file tf.cpp.

{
    return createPoseQ(translation, Eigen::Quaterniond(rotation));
}

createPoseQ() [3/3]

RobotPose robowflex::TF::createPoseQ	(	double	x,
		double	y,
		double	z,
		double	W,
		double	X,
		double	Y,
		double	Z
	)

Creates a robot pose from a linear component and a Quaternion.

Parameters

[in]	x	X-axis translation.
[in]	y	Y-axis translation.
[in]	z	Z-axis translation.
[in]	W	Real quaternion component.
[in]	X	i quaternion component.
[in]	Y	j quaternion component.
[in]	Z	k quaternion component.

Returns

A new robot pose from components.

Definition at line 47 of file tf.cpp.

{
    return createPoseQ(Eigen::Vector3d{x, y, z},  //
                       Eigen::Vector4d{W, X, Y, Z});
}

createPoseXYZ() [1/4]

RobotPose robowflex::TF::createPoseXYZ

(

const Eigen::Ref< const Eigen::Vector3d > &

translation

)

Creates a robot pose from a linear component and zero orientation.

Parameters

[in]

translation

Translation component.

Returns

A new robot pose from components.

Definition at line 19 of file tf.cpp.

{
    RobotPose pose = RobotPose::Identity();
    pose.translation() = translation;
 
    return pose;
}

createPoseXYZ() [2/4]

RobotPose robowflex::TF::createPoseXYZ	(	const Eigen::Ref< const Eigen::Vector3d > &	translation,
		const Eigen::Ref< const Eigen::Vector3d > &	rotation
	)

Creates a robot pose from a linear component and XYZ convention Euler angles.

Parameters

[in]	translation	Translation component.
[in]	rotation	Rotational component (X, Y, Z angles).

Returns

A new robot pose from components.

Definition at line 33 of file tf.cpp.

{
    RobotPose pose = RobotPose::Identity();
    pose.translation() = translation;
 
    pose.linear() = (Eigen::AngleAxisd(rotation[0], Eigen::Vector3d::UnitX()) *
                     Eigen::AngleAxisd(rotation[1], Eigen::Vector3d::UnitY()) *
                     Eigen::AngleAxisd(rotation[2], Eigen::Vector3d::UnitZ()))
                        .toRotationMatrix();
 
    return pose;
}

createPoseXYZ() [3/4]

RobotPose robowflex::TF::createPoseXYZ	(	double	x,
		double	y,
		double	z
	)

Creates a robot pose from a linear component and zero orientation.

Parameters

[in]	x	X-axis translation.
[in]	y	Y-ayis translation.
[in]	z	Z-azis translation.

Returns

A new robot pose from components.

Definition at line 14 of file tf.cpp.

{
    return createPoseXYZ(x, y, z, 0, 0, 0);
}

createPoseXYZ() [4/4]

RobotPose robowflex::TF::createPoseXYZ	(	double	x,
		double	y,
		double	z,
		double	X,
		double	Y,
		double	Z
	)

Creates a robot pose from a linear component and XYZ convention Euler angles.

Parameters

[in]	x	X-axis translation.
[in]	y	Y-ayis translation.
[in]	z	Z-azis translation.
[in]	X	Rotation about X.
[in]	Y	Rotation about Y.
[in]	Z	Rotation about Z.

Returns

A new robot pose from components.

Definition at line 27 of file tf.cpp.

{
    return createPoseXYZ(Eigen::Vector3d{x, y, z},  //
                         Eigen::Vector3d{X, Y, Z});
}

getBoundingVolume()

moveit_msgs::BoundingVolume robowflex::TF::getBoundingVolume	(	const RobotPose &	pose,
		const GeometryConstPtr &	geometry
	)

Get a bounding volume message for given geometry at a pose.

Parameters

[in]	pose	Pose to place geometry at.
[in]	geometry	Geometry to get bounding volume for.

Returns

Bounding volume message for geometry at pose.

Definition at line 151 of file tf.cpp.

{
    moveit_msgs::BoundingVolume bv;
 
    if (geometry->isMesh())
    {
        bv.meshes.push_back(geometry->getMeshMsg());
        bv.mesh_poses.push_back(TF::poseEigenToMsg(pose));
    }
    else
    {
        bv.primitives.push_back(geometry->getSolidMsg());
        bv.primitive_poses.push_back(TF::poseEigenToMsg(pose));
    }
 
    return bv;
}

getOrientationConstraint()

moveit_msgs::OrientationConstraint robowflex::TF::getOrientationConstraint	(	const std::string &	ee_name,
		const std::string &	base_name,
		const Eigen::Quaterniond &	orientation,
		const Eigen::Vector3d &	tolerances
	)

Get an orientation constraint message.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The frame of pose and orientation.
[in]	orientation	The desired orientation.
[in]	tolerances	XYZ Euler angle tolerances about orientation.

Returns

The orientation constraint as a MoveIt message.

Definition at line 189 of file tf.cpp.

{
    moveit_msgs::OrientationConstraint constraint;
 
    constraint.header.frame_id = base_name;
    constraint.link_name = ee_name;
    constraint.absolute_x_axis_tolerance = tolerances[0];
    constraint.absolute_y_axis_tolerance = tolerances[1];
    constraint.absolute_z_axis_tolerance = tolerances[2];
    constraint.orientation = TF::quaternionEigenToMsg(orientation);
    constraint.weight = 1;
 
    return constraint;
}

getPoseRotation()

Eigen::Quaterniond robowflex::TF::getPoseRotation

(

const RobotPose &

pose

)

Get the rotational component of a robot pose.

Parameters

[in]

pose

The pose to get the rotation from.

Returns

The rotational component of the pose.

Definition at line 69 of file tf.cpp.

{
    return Eigen::Quaterniond(pose.rotation());
}

getPositionConstraint()

moveit_msgs::PositionConstraint robowflex::TF::getPositionConstraint	(	const std::string &	ee_name,
		const std::string &	base_name,
		const RobotPose &	pose,
		const GeometryConstPtr &	geometry
	)

Get a position constraint message.

Parameters

[in]	ee_name	The name of the end-effector link.
[in]	base_name	The frame of pose and orientation.
[in]	pose	The pose of geometry in base_frame.
[in]	geometry	The geometry describing the position constraint.

Returns

The position constraint as a MoveIt messsage.

Definition at line 169 of file tf.cpp.

{
    moveit_msgs::PositionConstraint constraint;
 
    constraint.header.frame_id = base_name;
    constraint.link_name = ee_name;
 
    // TODO: Expose as a parameter
    constraint.target_point_offset.x = 0;
    constraint.target_point_offset.y = 0;
    constraint.target_point_offset.z = 0;
 
    constraint.constraint_region = getBoundingVolume(pose, geometry);
    constraint.weight = 1;
 
    return constraint;
}

identity()

RobotPose robowflex::TF::identity

(

)

Creates the Identity pose.

Returns

A new identity robot pose.

Definition at line 9 of file tf.cpp.

{
    return RobotPose::Identity();
}

isVecZero()

bool robowflex::TF::isVecZero	(	const Eigen::Ref< const Eigen::VectorXd > &	v,
		double	tolerance = constants::eps
	)

Checks if a vector is close to zero.

Parameters

[in]	v	Vector to check.
[in]	tolerance	Tolerance of what is considered zero.

Returns

Whether the vector's norm is below the tolerance threshold.

Definition at line 321 of file tf.cpp.

{
    return v.norm() < tolerance;
}

offsetOrientation()

Eigen::Quaterniond robowflex::TF::offsetOrientation	(	const Eigen::Quaterniond &	orientation,
		const Eigen::Vector3d &	axis,
		double	value
	)

Offset an orientation by a rotation about an axis.

Parameters

[in]	orientation	Orientation to offset.
[in]	axis	Axis to offset orientation about.
[in]	value	Value by which to offset.

Returns

The new orientation.

Definition at line 228 of file tf.cpp.

{
    return Eigen::AngleAxisd(value, axis) * orientation;
}

pointEigenToMsg()

geometry_msgs::Point robowflex::TF::pointEigenToMsg

(

const Eigen::Vector3d &

vector

)

Converts an Eigen::Vector3d to a point message.

Parameters

[in]

vector

Vector to convert.

Returns

vector as a point message.

Definition at line 84 of file tf.cpp.

{
    geometry_msgs::Point msg;
    msg.x = vector[0];
    msg.y = vector[1];
    msg.z = vector[2];
 
    return msg;
}

pointMsgToEigen()

Eigen::Vector3d robowflex::TF::pointMsgToEigen

(

const geometry_msgs::Point &

msg

)

Converts a point message to an Eigen::Vector3d.

Parameters

[in]

msg

Message to convert.

Returns

msg as an Eigen::Vector3d.

Definition at line 74 of file tf.cpp.

{
    Eigen::Vector3d vector;
    vector[0] = msg.x;
    vector[1] = msg.y;
    vector[2] = msg.z;
 
    return vector;
}

poseEigenToMsg()

geometry_msgs::Pose robowflex::TF::poseEigenToMsg

(

const RobotPose &

pose

)

Converts an RobotPose to a pose message.

Parameters

[in]

pose

Pose to convert.

Returns

pose as a pose message.

Definition at line 120 of file tf.cpp.

{
    geometry_msgs::Pose msg;
 
    const auto &t = pose.translation();
    msg.position.x = t.x();
    msg.position.y = t.y();
    msg.position.z = t.z();
 
    const auto &r = getPoseRotation(pose);
    msg.orientation = quaternionEigenToMsg(r);
 
    return msg;
}

poseMsgToEigen()

RobotPose robowflex::TF::poseMsgToEigen

(

const geometry_msgs::Pose &

msg

)

Converts a pose message to RobotPose.

Parameters

[in]

msg

Message to convert.

Returns

msg an RobotPose.

Definition at line 114 of file tf.cpp.

{
    return RobotPose(Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
                     quaternionMsgToEigen(msg.orientation));
}

quaternionEigenToMsg()

geometry_msgs::Quaternion robowflex::TF::quaternionEigenToMsg

(

const Eigen::Quaterniond &

quaternion

)

Converts an Eigen::Quaterniond to a quaternion message.

Parameters

[in]

quaternion

Quaternion to convert.

Returns

quaternion as a quaternion message.

Definition at line 140 of file tf.cpp.

{
    geometry_msgs::Quaternion msg;
    msg.w = quaternion.w();
    msg.x = quaternion.x();
    msg.y = quaternion.y();
    msg.z = quaternion.z();
 
    return msg;
}

quaternionMsgToEigen()

Eigen::Quaterniond robowflex::TF::quaternionMsgToEigen

(

const geometry_msgs::Quaternion &

msg

)

Converts a quaternion message to Eigen::Quaterniond.

Parameters

[in]

msg

Message to convert.

Returns

msg an Eigen::Quaterniond.

Definition at line 135 of file tf.cpp.

{
    return Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z);
}

sampleOrientation()

Eigen::Quaterniond robowflex::TF::sampleOrientation	(	const Eigen::Quaterniond &	orientation,
		const Eigen::Vector3d &	tolerances
	)

Sample an orientation from a given orientation with XYZ Euler angle tolerances.

Parameters

[in]	orientation	The desired mean orientation.
[in]	tolerances	XYZ Euler angle tolerances about orientation.

Returns

The sampled orientation.

Definition at line 207 of file tf.cpp.

{
    const auto vec = RNG::uniformVec(tolerances);
    Eigen::Quaterniond sampled = Eigen::AngleAxisd(vec[0], Eigen::Vector3d::UnitX())    //
                                 * Eigen::AngleAxisd(vec[1], Eigen::Vector3d::UnitY())  //
                                 * Eigen::AngleAxisd(vec[2], Eigen::Vector3d::UnitZ());
 
    return orientation * sampled;
}

sampleOrientationUniform()

Eigen::Quaterniond robowflex::TF::sampleOrientationUniform

(

const Eigen::Vector3d &

tolerances

)

Sample an orientation within the XYZ Euler angle tolerances.

Parameters

[in]

tolerances

XYZ Euler angle tolerances about orientation.

Returns

The sampled orientation.

Definition at line 218 of file tf.cpp.

{
    const auto vec = RNG::uniformRPY(tolerances);
    Eigen::Quaterniond sampled = Eigen::AngleAxisd(vec[0], Eigen::Vector3d::UnitX())    //
                                 * Eigen::AngleAxisd(vec[1], Eigen::Vector3d::UnitY())  //
                                 * Eigen::AngleAxisd(vec[2], Eigen::Vector3d::UnitZ());
 
    return sampled;
}

samplePoseGaussian()

RobotPose robowflex::TF::samplePoseGaussian	(	const Eigen::Vector3d &	pos_stddev,
		const Eigen::Vector3d &	orn_bounds
	)

Sample a pose with gaussian sampling for the position with given standard deviations and uniform sampling for the orientation within the given bounds.

Parameters

[in]	pos_stddev	The desired position standard deviations.
[in]	orn_bounds	The desired orientation bounds.

Returns

The sampled pose.

Definition at line 253 of file tf.cpp.

{
    auto sampled = RobotPose::Identity();
    sampled.translation() = samplePositionGaussian(pos_stddev);
    sampled.linear() = sampleOrientationUniform(orn_bounds).toRotationMatrix();
 
    return sampled;
}

samplePoseUniform()

RobotPose robowflex::TF::samplePoseUniform	(	const Eigen::Vector3d &	pos_bounds,
		const Eigen::Vector3d &	orn_bounds
	)

Sample a pose within the given position, orientation bounds.

Parameters

[in]	pos_bounds	The desired position bounds.
[in]	orn_bounds	The desired orientation bounds.

Returns

The sampled pose.

Definition at line 244 of file tf.cpp.

{
    auto sampled = RobotPose::Identity();
    sampled.translation() = samplePositionUniform(pos_bounds);
    sampled.linear() = sampleOrientationUniform(orn_bounds).toRotationMatrix();
 
    return sampled;
}

samplePositionConstraint()

Eigen::Vector3d robowflex::TF::samplePositionConstraint

(

const moveit_msgs::PositionConstraint &

pc

)

samplePositionGaussian()

Eigen::Vector3d robowflex::TF::samplePositionGaussian

(

const Eigen::Vector3d &

stddev

)

Sample a position from a gaussian distribution with mean zero and given standard deviation.

Parameters

[in]

stddev

The desired standard deviation for the position.

Returns

The sampled position.

Definition at line 239 of file tf.cpp.

{
    return RNG::gaussianVec(stddev);
}

samplePositionUniform()

Eigen::Vector3d robowflex::TF::samplePositionUniform

(

const Eigen::Vector3d &

bounds

)

Sample a position within the given bounds using a uniform distribution.

Parameters

[in]

bounds

The desired mean orientation.

Returns

The sampled position.

Definition at line 234 of file tf.cpp.

{
    return RNG::uniformVec(bounds);
}

toDegrees()

double robowflex::TF::toDegrees

(

double

v

)

Convert an angle to degrees.

Parameters

[in]

v

The angle in radians.

Returns

The angle in degrees.

Definition at line 298 of file tf.cpp.

{
    double n = angleNormalize(v);
    double d = n * 180. / constants::pi;
    if (n >= 0)
        return d;
 
    return 360. + d;
}

toRadians()

double robowflex::TF::toRadians

(

double

v

)

Convert an angle to radians.

Parameters

[in]

v

The angle in degrees.

Returns

The angle in radians.

Definition at line 308 of file tf.cpp.

{
    if (v < 0)
        v += 360.;
    if (v >= 360.)
        v -= 360.;
 
    if (v <= 180.)
        return v * constants::pi / 180.;
 
    return -(360. - v) * constants::pi / 180.;
}

transformEigenToMsg()

geometry_msgs::TransformStamped robowflex::TF::transformEigenToMsg	(	const std::string &	source,
		const std::string &	target,
		const RobotPose &	tf
	)

Encode a transform as a message.

Parameters

[in]	source	Source frame.
[in]	target	Target frame.
[in]	tf	Transform between frames.

Returns

Transform message.

Definition at line 262 of file tf.cpp.

{
    geometry_msgs::TransformStamped msg;
 
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = source;
    msg.child_frame_id = target;
 
    const auto &t = tf.translation();
    msg.transform.translation.x = t.x();
    msg.transform.translation.y = t.y();
    msg.transform.translation.z = t.z();
 
    const auto &r = TF::getPoseRotation(tf);
    msg.transform.rotation = quaternionEigenToMsg(r);
 
    return msg;
}

transformMsgToEigen()

RobotPose robowflex::TF::transformMsgToEigen

(

const geometry_msgs::TransformStamped &

tf

)

Decode a message as a transform.

Parameters

[in]

tf

Transform message.

Returns

The transform.

Definition at line 282 of file tf.cpp.

{
    RobotPose pose;
    pose.translation().x() = tf.transform.translation.x;
    pose.translation().y() = tf.transform.translation.y;
    pose.translation().z() = tf.transform.translation.z;
 
    pose.linear() = quaternionMsgToEigen(tf.transform.rotation).toRotationMatrix();
    return pose;
}

vectorEigenToMsg()

geometry_msgs::Vector3 robowflex::TF::vectorEigenToMsg

(

const Eigen::Vector3d &

vector

)

Converts an Eigen::Vector3d to a vector message.

Parameters

[in]

vector

Vector to convert.

Returns

vector as a vector message.

Definition at line 104 of file tf.cpp.

{
    geometry_msgs::Vector3 msg;
    msg.x = vector[0];
    msg.y = vector[1];
    msg.z = vector[2];
 
    return msg;
}

vectorMsgToEigen()

Eigen::Vector3d robowflex::TF::vectorMsgToEigen

(

const geometry_msgs::Vector3 &

msg

)

Converts a vector message to an Eigen::Vector3d.

Parameters

[in]

msg

Message to convert.

Returns

msg as an Eigen::Vector3d.

Definition at line 94 of file tf.cpp.

{
    Eigen::Vector3d vector;
    vector[0] = msg.x;
    vector[1] = msg.y;
    vector[2] = msg.z;
 
    return vector;
}